├── .gitignore ├── CMakeLists.txt ├── Makefile ├── README.md ├── data └── grid2d.pdf ├── doc ├── Experiment1.odt └── Experiment2.odt ├── launch ├── camera_cal.launch ├── camera_dot_tracker.launch ├── camera_lego_tracker.launch ├── camera_tracker.launch └── camera_view.launch ├── mainpage.dox ├── manifest.xml ├── models ├── dot_grid │ ├── dot_grid.init │ ├── dot_grid.wrl │ └── tracker.yaml ├── laas-box │ ├── laas-box.init │ ├── laas-box.wrl │ └── tracker.yaml └── lego │ ├── lego.init │ ├── lego.wrl │ └── tracker.yaml ├── src ├── callbacks.cpp ├── callbacks.hh ├── conversion.cpp ├── conversion.hh ├── homography_test.cpp └── visp_dot_tracker.cpp └── todo.txt /.gitignore: -------------------------------------------------------------------------------- 1 | bin/ 2 | build/ 3 | 4 | 5 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | rosbuild_init() 13 | 14 | #set the default path for built executables to the "bin" directory 15 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | #set the default path for built libraries to the "lib" directory 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | #uncomment if you have defined messages 20 | #rosbuild_genmsg() 21 | #uncomment if you have defined services 22 | #rosbuild_gensrv() 23 | 24 | #common commands for building c++ executables and libraries 25 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 26 | #target_link_libraries(${PROJECT_NAME} another_library) 27 | #rosbuild_add_boost_directories() 28 | #rosbuild_link_boost(${PROJECT_NAME} thread) 29 | #rosbuild_add_executable(example examples/example.cpp) 30 | #target_link_libraries(example ${PROJECT_NAME}) 31 | 32 | rosbuild_add_boost_directories() 33 | 34 | # Make sure Boost.Filesystem v2 is used. 35 | add_definitions(-DBOOST_FILESYSTEM_VERSION=2) 36 | 37 | rosbuild_add_executable(DotTracker src/visp_dot_tracker.cpp src/callbacks.cpp src/conversion.cpp) 38 | 39 | rosbuild_add_executable(HomographyTest src/homography_test.cpp) 40 | #target_link_libraries(visp_bridge visp-2) 41 | 42 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | linuxCNC-to-ROS 2 | =============== 3 | 4 | connects linuxCNC software to the robot operating system -------------------------------------------------------------------------------- /data/grid2d.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomlarkworthy/linuxCNC_ROS/ab28460ce68c0d2196e507e3800da5313de4c2f5/data/grid2d.pdf -------------------------------------------------------------------------------- /doc/Experiment1.odt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomlarkworthy/linuxCNC_ROS/ab28460ce68c0d2196e507e3800da5313de4c2f5/doc/Experiment1.odt -------------------------------------------------------------------------------- /doc/Experiment2.odt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomlarkworthy/linuxCNC_ROS/ab28460ce68c0d2196e507e3800da5313de4c2f5/doc/Experiment2.odt -------------------------------------------------------------------------------- /launch/camera_cal.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | [0.0, 0.03, 0.06, 0.09, 0.12, 0.15, 0.0, 0.03, 0.06, 0.09, 0.12, 0.15, 0.0, 0.03, 0.06, 0.09, 0.12, 0.15, 0.0, 0.03, 0.06, 0.09, 0.12, 0.15, 0.00, 0.03, 0.06, 0.09, 0.12, 0.15, 0.0, 0.03, 0.06, 0.09, 0.12, 0.15] 26 | [0.0, 0.00, 0.00, 0.00, 0.00, 0.00, .03, 0.03, 0.03, 0.03, 0.03, 0.03, .06, 0.06, 0.06, 0.06, 0.06, 0.06, 0.09,0.09, 0.09, 0.09, 0.09, 0.09, 0.12, 0.12, 0.12, 0.12, 0.12, 0.12, 0.15,0.15, 0.15, 0.15, 0.15, 0.15] 27 | [0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.0, 0.00, 0.00, 0.00, 0.00, 0.00] 28 | 29 | 30 | [0.03, 0.03, 0.09, 0.12] 31 | [0.03, 0.12, 0.12, 0.03] 32 | [0.00, 0.00, 0.00, 0.00] 33 | 34 | 35 | 36 | 37 | 40 | 41 | 48 | 49 | 133 | 134 | 135 | -------------------------------------------------------------------------------- /launch/camera_dot_tracker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | [0.03983109092, 0.0, 0.0, 0.0, 0.0] 17 | [666.015717, 0.000000, 319.2997418, 0.000000, 662.0900984, 246.9209726, 0.000000, 0.000000, 1.000000] 18 | [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] 19 | [666.015717, 0.000000, 319.2997418, 0.000000, 0.000000, 662.0900984, 246.9209726, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | [0.0, 0.03, 0.06, 0.09, 0.12, 0.15, 0.0, 0.03, 0.06, 0.09, 0.12, 0.15, 0.0, 0.03, 0.06, 0.09, 0.12, 0.15, 0.0, 0.03, 0.06, 0.09, 0.12, 0.15, 0.00, 0.03, 0.06, 0.09, 0.12, 0.15, 0.0, 0.03, 0.06, 0.09, 0.12, 0.15] 37 | [0.0, 0.00, 0.00, 0.00, 0.00, 0.00, .03, 0.03, 0.03, 0.03, 0.03, 0.03, .06, 0.06, 0.06, 0.06, 0.06, 0.06, 0.09,0.09, 0.09, 0.09, 0.09, 0.09, 0.12, 0.12, 0.12, 0.12, 0.12, 0.12, 0.15,0.15, 0.15, 0.15, 0.15, 0.15] 38 | [0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.0, 0.00, 0.00, 0.00, 0.00, 0.00] 39 | 40 | [0.03, 0.03, 0.09, 0.12] 41 | [0.03, 0.12, 0.12, 0.03] 42 | [0.00, 0.00, 0.00, 0.00] 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /launch/camera_lego_tracker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | [0.03983109092, 0.0, 0.0, 0.0, 0.0] 18 | [666.015717, 0.000000, 319.2997418, 0.000000, 662.0900984, 246.9209726, 0.000000, 0.000000, 1.000000] 19 | [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] 20 | [666.015717, 0.000000, 319.2997418, 0.000000, 0.000000, 662.0900984, 246.9209726, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /launch/camera_tracker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | [0.032095, -0.238155, 0.000104, -0.002138, 0.0000] 17 | [723.715912, 0.000000, 318.180121, 0.000000, 719.919423, 280.697379, 0.000000, 0.000000, 1.000000] 18 | [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] 19 | [723.715912, 0.000000, 318.180121, 0.000000, 0.000000, 719.919423, 280.697379, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /launch/camera_view.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b usb_vision_servo is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | usb_vision_servo 5 | 6 | 7 | tom 8 | BSD 9 | 10 | http://ros.org/wiki/usb_vision_servo 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /models/dot_grid/dot_grid.init: -------------------------------------------------------------------------------- 1 | 4 2 | 0 0 0, 3 | 0 0.08 0, 4 | 0.06 0 0 5 | 0.06 5 0 6 | -------------------------------------------------------------------------------- /models/dot_grid/dot_grid.wrl: -------------------------------------------------------------------------------- 1 | #VRML V2.0 utf8 2 | 3 | Group { 4 | children [ 5 | 6 | # Object "mesh" 7 | Shape { 8 | appearance Appearance { 9 | material Material { 10 | diffuseColor 0.8 0.8 0.8 11 | specularColor 0.8 0.8 0.8 12 | emissiveColor 0 0 0 13 | shininess 1.000000 14 | transparency 0.000000 15 | } 16 | } 17 | geometry IndexedFaceSet { 18 | solid FALSE 19 | creaseAngle 3.14159 20 | coord Coordinate { 21 | point [ 22 | 0 0 0, 23 | 0 0.08 0, 24 | 0.06 0 0 25 | ] 26 | } 27 | 28 | coordIndex [ 29 | 1,2,3, -1]} 30 | } 31 | ] 32 | } 33 | -------------------------------------------------------------------------------- /models/dot_grid/tracker.yaml: -------------------------------------------------------------------------------- 1 | { 2 | mask_size: 3, 3 | mu1: 0.5, 4 | mu2: 0.5, 5 | n_mask: 180, 6 | ntotal_sample: 800, 7 | range: 10, 8 | sample_step: 3., 9 | threshold: 2000.0 10 | } 11 | -------------------------------------------------------------------------------- /models/laas-box/laas-box.init: -------------------------------------------------------------------------------- 1 | 4 2 | -0.012 0.022 0.0315 3 | -0.012 -0.022 0.0315 4 | 0.012 -0.022 0.0315 5 | -0.012 -0.022 -0.0315 6 | -------------------------------------------------------------------------------- /models/laas-box/laas-box.wrl: -------------------------------------------------------------------------------- 1 | #VRML V2.0 utf8 2 | 3 | Group { 4 | children [ 5 | 6 | # Object "box" 7 | Shape { 8 | appearance Appearance { 9 | material Material { 10 | diffuseColor 1 1 1 11 | specularColor 0 0 0 12 | emissiveColor 0 0 0 13 | shininess 0.078125 14 | transparency 0.000000 15 | } 16 | } 17 | geometry IndexedFaceSet { 18 | solid FALSE 19 | creaseAngle 0.785398 20 | coord Coordinate { 21 | point [ 22 | -0.012 -0.022 -0.0315, 23 | 0.012 -0.022 -0.0315, 24 | 0.012 -0.022 0.0315, 25 | -0.012 -0.022 0.0315, 26 | -0.012 0.022 0.0315, 27 | 0.012 0.022 0.0315, 28 | 0.012 0.022 -0.0315, 29 | -0.012 0.022 -0.0315 30 | ] 31 | } 32 | 33 | 34 | coordIndex [ 35 | 0, 1, 2, 3, -1, 36 | 7, 4, 5, 6, -1, 37 | 4, 3, 2, 5, -1, 38 | 6, 1, 0, 7, -1, 39 | 7, 0, 3, 4, -1, 40 | 5, 2, 1, 6, -1]} 41 | } 42 | ] 43 | } 44 | -------------------------------------------------------------------------------- /models/laas-box/tracker.yaml: -------------------------------------------------------------------------------- 1 | { 2 | mask_size: 3, 3 | mu1: 0.5, 4 | mu2: 0.5, 5 | n_mask: 180, 6 | ntotal_sample: 800, 7 | range: 10, 8 | sample_step: 3., 9 | threshold: 2000.0 10 | } 11 | -------------------------------------------------------------------------------- /models/lego/lego.init: -------------------------------------------------------------------------------- 1 | 8 2 | -.499 -0.499 .155 3 | -.499 0.499 .155 4 | .499 0.499 .155 5 | .499 -0.499 .155 6 | 7 | -.391 -0.391 -.155 8 | -.391 0.391 -.155 9 | .391 0.391 -.155 10 | .391 -0.391 -.155 11 | -------------------------------------------------------------------------------- /models/lego/lego.wrl: -------------------------------------------------------------------------------- 1 | #VRML V2.0 utf8 2 | 3 | Group { 4 | children [ 5 | 6 | # Object "box" 7 | Shape { 8 | appearance Appearance { 9 | material Material { 10 | diffuseColor 1 1 1 11 | specularColor 0 0 0 12 | emissiveColor 0 0 0 13 | shininess 0.078125 14 | transparency 0.000000 15 | } 16 | } 17 | geometry IndexedFaceSet { 18 | solid FALSE 19 | creaseAngle 0.785398 20 | coord Coordinate { 21 | point [ 22 | -0.119995117188 -0.0400085449219 0.154495239258 23 | -0.119995117188 0.0399932861328 0.154495239258 24 | -0.0399932861328 0.0399932861328 0.154495239258 25 | -0.0399932861328 -0.0400085449219 0.154495239258 26 | -0.119995117188 0.0399932861328 0.146499633789 27 | -0.119995117188 -0.0400085449219 0.146499633789 28 | -0.0399932861328 -0.0400085449219 0.146499633789 29 | -0.0399932861328 0.0399932861328 0.146499633789 30 | 0.120002746582 0.0399932861328 0.154495239258 31 | 0.120002746582 -0.0400085449219 0.154495239258 32 | 0.0400047302246 -0.0400085449219 0.154495239258 33 | 0.0400047302246 0.0399932861328 0.154495239258 34 | 0.120002746582 -0.0400085449219 0.146499633789 35 | 0.120002746582 0.0399932861328 0.146499633789 36 | 0.0400047302246 0.0399932861328 0.146499633789 37 | 0.0400047302246 -0.0400085449219 0.146499633789 38 | 0.0400047302246 -0.120010375977 0.154495239258 39 | -0.0399932861328 -0.120010375977 0.154495239258 40 | -0.0399932861328 -0.0400085449219 0.154495239258 41 | 0.0400047302246 -0.0400085449219 0.154495239258 42 | -0.0399932861328 -0.120010375977 0.146499633789 43 | 0.0400047302246 -0.120010375977 0.146499633789 44 | 0.0400047302246 -0.0400085449219 0.146499633789 45 | -0.0399932861328 -0.0400085449219 0.146499633789 46 | -0.0399932861328 0.119995117188 0.154495239258 47 | 0.0400047302246 0.119995117188 0.154495239258 48 | 0.0400047302246 0.0399932861328 0.154495239258 49 | -0.0399932861328 0.0399932861328 0.154495239258 50 | 0.0400047302246 0.119995117188 0.146499633789 51 | -0.0399932861328 0.119995117188 0.146499633789 52 | -0.0399932861328 0.0399932861328 0.146499633789 53 | 0.0400047302246 0.0399932861328 0.146499633789 54 | -0.390991210938 -0.450012207031 -0.154495239258 55 | -0.390991210938 -0.429992675781 -0.154495239258 56 | -0.408996582031 -0.429992675781 -0.154495239258 57 | -0.408996582031 -0.450012207031 -0.154495239258 58 | -0.390991210938 -0.450012207031 0.125495910645 59 | -0.408996582031 -0.450012207031 0.125495910645 60 | -0.408996582031 -0.429992675781 0.125495910645 61 | -0.390991210938 -0.429992675781 0.125495910645 62 | -0.390991210938 -0.429992675781 -0.154495239258 63 | -0.408996582031 -0.429992675781 -0.154495239258 64 | -0.390991210938 -0.429992675781 0.125495910645 65 | -0.408996582031 -0.429992675781 0.125495910645 66 | 0.408996582031 -0.450012207031 -0.154495239258 67 | 0.408996582031 -0.429992675781 -0.154495239258 68 | 0.390991210938 -0.429992675781 -0.154495239258 69 | 0.390991210938 -0.450012207031 -0.154495239258 70 | 0.408996582031 -0.450012207031 0.125495910645 71 | 0.390991210938 -0.450012207031 0.125495910645 72 | 0.390991210938 -0.429992675781 0.125495910645 73 | 0.408996582031 -0.429992675781 0.125495910645 74 | 0.408996582031 -0.429992675781 -0.154495239258 75 | 0.390991210938 -0.429992675781 -0.154495239258 76 | 0.408996582031 -0.429992675781 0.125495910645 77 | 0.390991210938 -0.429992675781 0.125495910645 78 | 0.408996582031 0.429992675781 -0.154495239258 79 | 0.408996582031 0.449981689453 -0.154495239258 80 | 0.390991210938 0.449981689453 -0.154495239258 81 | 0.390991210938 0.429992675781 -0.154495239258 82 | 0.408996582031 0.429992675781 0.125495910645 83 | 0.390991210938 0.429992675781 0.125495910645 84 | 0.390991210938 0.449981689453 0.125495910645 85 | 0.408996582031 0.449981689453 0.125495910645 86 | 0.408996582031 0.429992675781 -0.154495239258 87 | 0.390991210938 0.429992675781 -0.154495239258 88 | 0.390991210938 0.429992675781 0.125495910645 89 | 0.408996582031 0.429992675781 0.125495910645 90 | -0.390991210938 0.429992675781 -0.154495239258 91 | -0.390991210938 0.449981689453 -0.154495239258 92 | -0.408996582031 0.449981689453 -0.154495239258 93 | -0.408996582031 0.429992675781 -0.154495239258 94 | -0.390991210938 0.429992675781 0.125495910645 95 | -0.408996582031 0.429992675781 0.125495910645 96 | -0.408996582031 0.449981689453 0.125495910645 97 | -0.390991210938 0.449981689453 0.125495910645 98 | -0.390991210938 0.429992675781 -0.154495239258 99 | -0.408996582031 0.429992675781 -0.154495239258 100 | -0.408996582031 0.429992675781 0.125495910645 101 | -0.390991210938 0.429992675781 0.125495910645 102 | -0.449981689453 0.390991210938 -0.154495239258 103 | -0.429992675781 0.390991210938 -0.154495239258 104 | -0.429992675781 0.408996582031 -0.154495239258 105 | -0.449981689453 0.408996582031 -0.154495239258 106 | -0.449981689453 0.390991210938 0.125495910645 107 | -0.449981689453 0.408996582031 0.125495910645 108 | -0.429992675781 0.408996582031 0.125495910645 109 | -0.429992675781 0.390991210938 0.125495910645 110 | -0.429992675781 0.390991210938 -0.154495239258 111 | -0.429992675781 0.408996582031 -0.154495239258 112 | -0.429992675781 0.390991210938 0.125495910645 113 | -0.429992675781 0.408996582031 0.125495910645 114 | -0.449981689453 -0.408996582031 -0.154495239258 115 | -0.429992675781 -0.408996582031 -0.154495239258 116 | -0.429992675781 -0.391021728516 -0.154495239258 117 | -0.449981689453 -0.391021728516 -0.154495239258 118 | -0.449981689453 -0.408996582031 0.125495910645 119 | -0.449981689453 -0.391021728516 0.125495910645 120 | -0.429992675781 -0.391021728516 0.125495910645 121 | -0.429992675781 -0.408996582031 0.125495910645 122 | -0.429992675781 -0.408996582031 -0.154495239258 123 | -0.429992675781 -0.391021728516 -0.154495239258 124 | -0.429992675781 -0.408996582031 0.125495910645 125 | -0.429992675781 -0.391021728516 0.125495910645 126 | 0.429992675781 -0.408996582031 -0.154495239258 127 | 0.450012207031 -0.408996582031 -0.154495239258 128 | 0.450012207031 -0.391021728516 -0.154495239258 129 | 0.429992675781 -0.391021728516 -0.154495239258 130 | 0.429992675781 -0.408996582031 0.125495910645 131 | 0.429992675781 -0.391021728516 0.125495910645 132 | 0.450012207031 -0.391021728516 0.125495910645 133 | 0.450012207031 -0.408996582031 0.125495910645 134 | 0.429992675781 -0.408996582031 -0.154495239258 135 | 0.429992675781 -0.391021728516 -0.154495239258 136 | 0.429992675781 -0.391021728516 0.125495910645 137 | 0.429992675781 -0.408996582031 0.125495910645 138 | 0.429992675781 0.390991210938 -0.154495239258 139 | 0.450012207031 0.390991210938 -0.154495239258 140 | 0.450012207031 0.408996582031 -0.154495239258 141 | 0.429992675781 0.408996582031 -0.154495239258 142 | 0.429992675781 0.390991210938 0.125495910645 143 | 0.429992675781 0.408996582031 0.125495910645 144 | 0.450012207031 0.408996582031 0.125495910645 145 | 0.450012207031 0.390991210938 0.125495910645 146 | 0.429992675781 0.390991210938 -0.154495239258 147 | 0.429992675781 0.408996582031 -0.154495239258 148 | 0.429992675781 0.408996582031 0.125495910645 149 | 0.429992675781 0.390991210938 0.125495910645 150 | 0.0300064086914 0.480987548828 0.115501403809 151 | 0.0300064086914 0.480987548828 0.0954971313477 152 | 0.0300064086914 0.498992919922 0.0954971313477 153 | 0.0300064086914 0.498992919922 0.115501403809 154 | 0.309997558594 0.480987548828 0.115501403809 155 | 0.309997558594 0.498992919922 0.115501403809 156 | 0.309997558594 0.498992919922 0.0954971313477 157 | 0.309997558594 0.480987548828 0.0954971313477 158 | 0.0300064086914 0.480987548828 0.115501403809 159 | 0.0300064086914 0.498992919922 0.115501403809 160 | 0.309997558594 0.498992919922 0.115501403809 161 | 0.309997558594 0.480987548828 0.115501403809 162 | -0.309997558594 0.480987548828 0.115501403809 163 | -0.309997558594 0.480987548828 0.0954971313477 164 | -0.309997558594 0.498992919922 0.0954971313477 165 | -0.309997558594 0.498992919922 0.115501403809 166 | -0.029993057251 0.480987548828 0.115501403809 167 | -0.029993057251 0.498992919922 0.115501403809 168 | -0.029993057251 0.498992919922 0.0954971313477 169 | -0.029993057251 0.480987548828 0.0954971313477 170 | -0.309997558594 0.480987548828 0.115501403809 171 | -0.309997558594 0.498992919922 0.115501403809 172 | -0.029993057251 0.498992919922 0.115501403809 173 | -0.029993057251 0.480987548828 0.115501403809 174 | 0.0300064086914 -0.498992919922 0.115501403809 175 | 0.0300064086914 -0.498992919922 0.0954971313477 176 | 0.0300064086914 -0.481018066406 0.0954971313477 177 | 0.0300064086914 -0.481018066406 0.115501403809 178 | 0.309997558594 -0.498992919922 0.115501403809 179 | 0.309997558594 -0.481018066406 0.115501403809 180 | 0.309997558594 -0.481018066406 0.0954971313477 181 | 0.309997558594 -0.498992919922 0.0954971313477 182 | 0.0300064086914 -0.498992919922 0.115501403809 183 | 0.0300064086914 -0.481018066406 0.115501403809 184 | 0.309997558594 -0.481018066406 0.115501403809 185 | 0.309997558594 -0.498992919922 0.115501403809 186 | -0.309997558594 -0.498992919922 0.115501403809 187 | -0.309997558594 -0.498992919922 0.0954971313477 188 | -0.309997558594 -0.481018066406 0.0954971313477 189 | -0.309997558594 -0.481018066406 0.115501403809 190 | -0.029993057251 -0.498992919922 0.115501403809 191 | -0.029993057251 -0.481018066406 0.115501403809 192 | -0.029993057251 -0.481018066406 0.0954971313477 193 | -0.029993057251 -0.498992919922 0.0954971313477 194 | -0.309997558594 -0.498992919922 0.115501403809 195 | -0.309997558594 -0.481018066406 0.115501403809 196 | -0.029993057251 -0.481018066406 0.115501403809 197 | -0.029993057251 -0.498992919922 0.115501403809 198 | -0.480987548828 0.029993057251 0.115501403809 199 | -0.498992919922 0.029993057251 0.115501403809 200 | -0.498992919922 0.309997558594 0.115501403809 201 | -0.480987548828 0.309997558594 0.115501403809 202 | -0.480987548828 0.029993057251 0.115501403809 203 | -0.480987548828 0.029993057251 0.0954971313477 204 | -0.498992919922 0.029993057251 0.0954971313477 205 | -0.498992919922 0.029993057251 0.115501403809 206 | -0.480987548828 0.309997558594 0.115501403809 207 | -0.498992919922 0.309997558594 0.115501403809 208 | -0.498992919922 0.309997558594 0.0954971313477 209 | -0.480987548828 0.309997558594 0.0954971313477 210 | -0.480987548828 -0.309997558594 0.115501403809 211 | -0.480987548828 -0.309997558594 0.0954971313477 212 | -0.498992919922 -0.309997558594 0.0954971313477 213 | -0.498992919922 -0.309997558594 0.115501403809 214 | -0.480987548828 -0.03000831604 0.115501403809 215 | -0.498992919922 -0.03000831604 0.115501403809 216 | -0.498992919922 -0.03000831604 0.0954971313477 217 | -0.480987548828 -0.03000831604 0.0954971313477 218 | -0.480987548828 -0.309997558594 0.115501403809 219 | -0.498992919922 -0.309997558594 0.115501403809 220 | -0.498992919922 -0.03000831604 0.115501403809 221 | -0.480987548828 -0.03000831604 0.115501403809 222 | 0.498992919922 0.029993057251 0.115501403809 223 | 0.498992919922 0.029993057251 0.0954971313477 224 | 0.481018066406 0.029993057251 0.0954971313477 225 | 0.481018066406 0.029993057251 0.115501403809 226 | 0.498992919922 0.309997558594 0.115501403809 227 | 0.481018066406 0.309997558594 0.115501403809 228 | 0.481018066406 0.309997558594 0.0954971313477 229 | 0.498992919922 0.309997558594 0.0954971313477 230 | 0.498992919922 0.029993057251 0.115501403809 231 | 0.481018066406 0.029993057251 0.115501403809 232 | 0.481018066406 0.309997558594 0.115501403809 233 | 0.498992919922 0.309997558594 0.115501403809 234 | 0.498992919922 -0.309997558594 0.115501403809 235 | 0.498992919922 -0.309997558594 0.0954971313477 236 | 0.481018066406 -0.309997558594 0.0954971313477 237 | 0.481018066406 -0.309997558594 0.115501403809 238 | 0.498992919922 -0.03000831604 0.115501403809 239 | 0.481018066406 -0.03000831604 0.115501403809 240 | 0.481018066406 -0.03000831604 0.0954971313477 241 | 0.498992919922 -0.03000831604 0.0954971313477 242 | 0.498992919922 -0.309997558594 0.115501403809 243 | 0.481018066406 -0.309997558594 0.115501403809 244 | 0.481018066406 -0.03000831604 0.115501403809 245 | 0.498992919922 -0.03000831604 0.115501403809 246 | ] 247 | } 248 | 249 | coordIndex [ 250 | 4 5 6 7 -1, 251 | 0 5 4 1 -1, 252 | 7 2 1 4 -1, 253 | 5 0 3 6 -1, 254 | 12 13 14 15 -1, 255 | 8 13 12 9 -1, 256 | 15 10 9 12 -1, 257 | 13 8 11 14 -1, 258 | 20 21 22 23 -1, 259 | 16 21 20 17 -1, 260 | 23 18 17 20 -1, 261 | 21 16 19 22 -1, 262 | 28 29 30 31 -1, 263 | 24 29 28 25 -1, 264 | 31 26 25 28 -1, 265 | 29 24 27 30 -1, 266 | 32 33 39 36 -1, 267 | 37 38 34 35 -1, 268 | 44 45 51 48 -1, 269 | 49 50 46 47 -1, 270 | 56 57 63 60 -1, 271 | 61 62 58 59 -1, 272 | 68 69 75 72 -1, 273 | 73 74 70 71 -1, 274 | 80 81 87 84 -1, 275 | 85 86 82 83 -1, 276 | 92 93 99 96 -1, 277 | 97 98 94 95 -1, 278 | 104 105 111 108 -1, 279 | 109 110 106 107 -1, 280 | 116 117 123 120 -1, 281 | 121 122 118 119 -1, 282 | 128 129 135 132 -1, 283 | 133 134 130 131 -1, 284 | 140 141 147 144 -1, 285 | 145 146 142 143 -1, 286 | 152 153 159 156 -1, 287 | 157 158 154 155 -1, 288 | 164 165 171 168 -1, 289 | 169 170 166 167 -1, 290 | 180 181 187 184 -1, 291 | 185 186 182 183 -1, 292 | 188 189 195 192 -1, 293 | 193 194 190 191 -1, 294 | 200 201 207 204 -1, 295 | 205 206 202 203 -1, 296 | 212 213 219 216 -1, 297 | 217 218 214 215 -1]} 298 | } 299 | ] 300 | } 301 | 302 | -------------------------------------------------------------------------------- /models/lego/tracker.yaml: -------------------------------------------------------------------------------- 1 | { 2 | mask_size: 3, 3 | mu1: 0.5, 4 | mu2: 0.5, 5 | n_mask: 180, 6 | ntotal_sample: 800, 7 | range: 10, 8 | sample_step: 3., 9 | threshold: 2000.0 10 | } 11 | -------------------------------------------------------------------------------- /src/callbacks.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include "conversion.hh" 8 | 9 | #include "callbacks.hh" 10 | 11 | void imageCallback(vpImage& image, 12 | const sensor_msgs::Image::ConstPtr& msg, 13 | const sensor_msgs::CameraInfoConstPtr& info) 14 | { 15 | try 16 | { 17 | rosImageToVisp(image, msg); 18 | } 19 | catch(std::exception& e) 20 | { 21 | ROS_ERROR_STREAM("dropping frame: " << e.what()); 22 | } 23 | } 24 | 25 | void imageCallback(vpImage& image, 26 | std_msgs::Header& header, 27 | sensor_msgs::CameraInfoConstPtr& info, 28 | const sensor_msgs::Image::ConstPtr& msg, 29 | const sensor_msgs::CameraInfoConstPtr& infoConst) 30 | { 31 | imageCallback(image, msg, info); 32 | header = msg->header; 33 | info = infoConst; 34 | } 35 | 36 | 37 | image_transport::CameraSubscriber::Callback 38 | bindImageCallback(vpImage& image, 39 | std_msgs::Header& header, 40 | sensor_msgs::CameraInfoConstPtr& info) 41 | { 42 | return boost::bind 43 | (imageCallback, 44 | boost::ref(image), boost::ref(header), boost::ref(info), _1, _2); 45 | } 46 | 47 | 48 | /* 49 | void reconfigureCallback(vpMbEdgeTracker& tracker, 50 | vpImage& I, 51 | vpMe& moving_edge, 52 | visp_tracker::MovingEdgeConfig& config, 53 | uint32_t level) 54 | { 55 | ROS_INFO("Reconfigure request received."); 56 | convertMovingEdgeConfigToVpMe(config, moving_edge, tracker); 57 | 58 | //FIXME: not sure if this is needed. 59 | moving_edge.initMask(); 60 | 61 | vpHomogeneousMatrix cMo; 62 | tracker.getPose(cMo); 63 | 64 | tracker.setMovingEdge(moving_edge); 65 | tracker.init(I, cMo); 66 | 67 | moving_edge.print(); 68 | }*/ 69 | -------------------------------------------------------------------------------- /src/callbacks.hh: -------------------------------------------------------------------------------- 1 | #ifndef VISP_TRACKER_CALLBACKS_HH 2 | # define VISP_TRACKER_CALLBACKS_HH 3 | # include 4 | # include 5 | # include 6 | # include 7 | # include 8 | # include 9 | 10 | /* 11 | # include 12 | */ 13 | 14 | void 15 | imageCallback(vpImage& image, 16 | const sensor_msgs::Image::ConstPtr& msg, 17 | const sensor_msgs::CameraInfoConstPtr& info); 18 | 19 | void 20 | imageCallback(vpImage& image, 21 | std_msgs::Header& header, 22 | sensor_msgs::CameraInfoConstPtr& info, 23 | const sensor_msgs::Image::ConstPtr& msg, 24 | const sensor_msgs::CameraInfoConstPtr& infoConst); 25 | 26 | image_transport::CameraSubscriber::Callback 27 | bindImageCallback(vpImage& image); 28 | 29 | image_transport::CameraSubscriber::Callback 30 | bindImageCallback(vpImage& image, 31 | std_msgs::Header& header, 32 | sensor_msgs::CameraInfoConstPtr& info); 33 | 34 | /* 35 | void reconfigureCallback(vpMbEdgeTracker& tracker, 36 | vpImage& I, 37 | vpMe& moving_edge, 38 | visp_tracker::MovingEdgeConfig& config, 39 | uint32_t level); 40 | */ 41 | #endif //! VISP_TRACKER_CALLBACKS_HH 42 | -------------------------------------------------------------------------------- /src/conversion.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | /* 17 | #define protected public 18 | # include 19 | #undef protected 20 | */ 21 | 22 | #include "conversion.hh" 23 | 24 | void rosImageToVisp(vpImage& dst, 25 | const sensor_msgs::Image::ConstPtr& src) 26 | { 27 | using sensor_msgs::image_encodings::RGB8; 28 | using sensor_msgs::image_encodings::RGBA8; 29 | using sensor_msgs::image_encodings::BGR8; 30 | using sensor_msgs::image_encodings::BGRA8; 31 | using sensor_msgs::image_encodings::MONO8; 32 | using sensor_msgs::image_encodings::MONO16; 33 | using sensor_msgs::image_encodings::numChannels; 34 | 35 | // Resize the image if necessary. 36 | if (src->width != dst.getWidth() || src->height != dst.getHeight()) 37 | { 38 | ROS_INFO 39 | ("dst is %dx%d but src size is %dx%d, resizing.", 40 | src->width, src->height, 41 | dst.getWidth (), dst.getHeight ()); 42 | dst.resize (src->height, src->width); 43 | } 44 | 45 | if(src->encoding == MONO8) 46 | memcpy(dst.bitmap, 47 | &src->data[0], 48 | dst.getHeight () * src->step * sizeof(unsigned char)); 49 | else if(src->encoding == RGB8 || src->encoding == RGBA8 50 | || src->encoding == BGR8 || src->encoding == BGRA8) 51 | { 52 | unsigned nc = numChannels(src->encoding); 53 | unsigned cEnd = 54 | (src->encoding == RGBA8 || src->encoding == BGRA8) ? nc - 1 : nc; 55 | 56 | for(unsigned i = 0; i < dst.getWidth (); ++i) 57 | for(unsigned j = 0; j < dst.getHeight (); ++j) 58 | { 59 | int acc = 0; 60 | for(unsigned c = 0; c < cEnd; ++c) 61 | acc += src->data[j * src->step + i * nc + c]; 62 | dst[j][i] = acc / nc; 63 | } 64 | } 65 | else 66 | { 67 | boost::format fmt("bad encoding '%1'"); 68 | fmt % src->encoding; 69 | throw std::runtime_error(fmt.str()); 70 | } 71 | } 72 | 73 | void vispImageToRos(sensor_msgs::Image& dst, 74 | const vpImage& src) 75 | { 76 | dst.width = src.getWidth(); 77 | dst.height = src.getHeight(); 78 | dst.encoding = sensor_msgs::image_encodings::MONO8; 79 | dst.step = src.getWidth(); 80 | dst.data.resize(dst.height * dst.step); 81 | for(unsigned i = 0; i < src.getWidth (); ++i) 82 | for(unsigned j = 0; j < src.getHeight (); ++j) 83 | dst.data[j * dst.step + i] = src[j][i]; 84 | } 85 | 86 | void vpHomogeneousMatrixToTransform(geometry_msgs::Transform& dst, 87 | const vpHomogeneousMatrix& src) 88 | { 89 | btMatrix3x3 rotation; 90 | btQuaternion quaternion; 91 | for(unsigned i = 0; i < 3; ++i) 92 | for(unsigned j = 0; j < 3; ++j) 93 | rotation[i][j] = src[i][j]; 94 | rotation.getRotation(quaternion); 95 | 96 | dst.translation.x = src[0][3]; 97 | dst.translation.y = src[1][3]; 98 | dst.translation.z = src[2][3]; 99 | 100 | dst.rotation.x = quaternion.x(); 101 | dst.rotation.y = quaternion.y(); 102 | dst.rotation.z = quaternion.z(); 103 | dst.rotation.w = quaternion.w(); 104 | } 105 | 106 | void transformToVpHomogeneousMatrix(vpHomogeneousMatrix& dst, 107 | const geometry_msgs::Transform& src) 108 | { 109 | btQuaternion quaternion 110 | (src.rotation.x, src.rotation.y, src.rotation.z, src.rotation.w); 111 | btMatrix3x3 rotation(quaternion); 112 | 113 | // Copy the rotation component. 114 | for(unsigned i = 0; i < 3; ++i) 115 | for(unsigned j = 0; j < 3; ++j) 116 | dst[i][j] = rotation[i][j]; 117 | 118 | // Copy the translation component. 119 | dst[0][3] = src.translation.x; 120 | dst[1][3] = src.translation.y; 121 | dst[2][3] = src.translation.z; 122 | } 123 | /* 124 | void convertMovingEdgeConfigToVpMe(const visp_tracker::MovingEdgeConfig& config, 125 | vpMe& moving_edge, 126 | vpMbEdgeTracker& tracker) 127 | { 128 | moving_edge.mask_size = config.mask_size; 129 | moving_edge.n_mask = config.n_mask; 130 | moving_edge.range = config.range; 131 | moving_edge.threshold = config.threshold; 132 | moving_edge.mu1 = config.mu1; 133 | moving_edge.mu2 = config.mu2; 134 | moving_edge.sample_step = config.sample_step; 135 | moving_edge.ntotal_sample = config.ntotal_sample; 136 | 137 | moving_edge.strip = config.strip; 138 | moving_edge.min_samplestep = config.min_samplestep; 139 | moving_edge.aberration = config.aberration; 140 | moving_edge.init_aberration = config.init_aberration; 141 | 142 | tracker.setLambda(config.lambda); 143 | tracker.setFirstThreshold(config.first_threshold); 144 | } 145 | 146 | void convertVpMeToMovingEdgeConfig(const vpMe& moving_edge, 147 | const vpMbEdgeTracker& tracker, 148 | visp_tracker::MovingEdgeConfig& config) 149 | { 150 | config.mask_size = moving_edge.mask_size; 151 | config.n_mask = moving_edge.n_mask; 152 | config.range = moving_edge.range; 153 | config.threshold = moving_edge.threshold; 154 | config.mu1 = moving_edge.mu1; 155 | config.mu2 = moving_edge.mu2; 156 | config.sample_step = moving_edge.sample_step; 157 | config.ntotal_sample = moving_edge.ntotal_sample; 158 | 159 | config.strip = moving_edge.strip; 160 | config.min_samplestep = moving_edge.min_samplestep; 161 | config.aberration = moving_edge.aberration; 162 | config.init_aberration = moving_edge.init_aberration; 163 | 164 | config.lambda = tracker.lambda; 165 | config.first_threshold = tracker.percentageGdPt; 166 | } 167 | 168 | void convertVpMeToInitRequest(const vpMe& moving_edge, 169 | const vpMbEdgeTracker& tracker, 170 | visp_tracker::Init& srv) 171 | { 172 | srv.request.moving_edge.mask_size = moving_edge.mask_size; 173 | srv.request.moving_edge.n_mask = moving_edge.n_mask; 174 | srv.request.moving_edge.range = moving_edge.range; 175 | srv.request.moving_edge.threshold = moving_edge.threshold; 176 | srv.request.moving_edge.mu1 = moving_edge.mu1; 177 | srv.request.moving_edge.mu2 = moving_edge.mu2; 178 | srv.request.moving_edge.sample_step = moving_edge.sample_step; 179 | srv.request.moving_edge.ntotal_sample = moving_edge.ntotal_sample; 180 | 181 | srv.request.moving_edge.strip = moving_edge.strip; 182 | srv.request.moving_edge.min_samplestep = moving_edge.min_samplestep; 183 | srv.request.moving_edge.aberration = moving_edge.aberration; 184 | srv.request.moving_edge.init_aberration = moving_edge.init_aberration; 185 | 186 | srv.request.moving_edge.lambda = tracker.lambda; 187 | srv.request.moving_edge.first_threshold = tracker.percentageGdPt; 188 | } 189 | 190 | void convertInitRequestToVpMe(const visp_tracker::Init::Request& req, 191 | vpMbEdgeTracker& tracker, 192 | vpMe& moving_edge) 193 | { 194 | moving_edge.mask_size = req.moving_edge.mask_size; 195 | moving_edge.n_mask = req.moving_edge.n_mask; 196 | moving_edge.range = req.moving_edge.range; 197 | moving_edge.threshold = req.moving_edge.threshold; 198 | moving_edge.mu1 = req.moving_edge.mu1; 199 | moving_edge.mu2 = req.moving_edge.mu2; 200 | moving_edge.sample_step = req.moving_edge.sample_step; 201 | moving_edge.ntotal_sample = req.moving_edge.ntotal_sample; 202 | 203 | moving_edge.strip = req.moving_edge.strip; 204 | moving_edge.min_samplestep = req.moving_edge.min_samplestep; 205 | moving_edge.aberration = req.moving_edge.aberration; 206 | moving_edge.init_aberration = req.moving_edge.init_aberration; 207 | 208 | tracker.setLambda(req.moving_edge.lambda); 209 | tracker.setFirstThreshold(req.moving_edge.first_threshold); 210 | } 211 | 212 | void initializeVpCameraFromCameraInfo(vpCameraParameters& cam, 213 | sensor_msgs::CameraInfoConstPtr info) 214 | { 215 | if (!info) 216 | throw std::runtime_error ("missing camera calibration data"); 217 | 218 | // Check that the camera is calibrated, as specified in the 219 | // sensor_msgs/CameraInfo message documentation. 220 | if (info->K.size() != 3 * 3 || info->K[0] == 0.) 221 | throw std::runtime_error ("uncalibrated camera"); 222 | 223 | // Check matrix size. 224 | if (!info || info->P.size() != 3 * 4) 225 | throw std::runtime_error 226 | ("camera calibration P matrix has an incorrect size"); 227 | 228 | if (info->distortion_model.empty ()) 229 | { 230 | const double& px = info->K[0 * 3 + 0]; 231 | const double& py = info->K[1 * 3 + 1]; 232 | const double& u0 = info->K[0 * 3 + 2]; 233 | const double& v0 = info->K[1 * 3 + 2]; 234 | cam.initPersProjWithoutDistortion(px, py, u0, v0); 235 | return; 236 | } 237 | 238 | if (info->distortion_model == sensor_msgs::distortion_models::PLUMB_BOB) 239 | { 240 | const double& px = info->P[0 * 4 + 0]; 241 | const double& py = info->P[1 * 4 + 1]; 242 | const double& u0 = info->P[0 * 4 + 2]; 243 | const double& v0 = info->P[1 * 4 + 2]; 244 | cam.initPersProjWithoutDistortion(px, py, u0, v0); 245 | return; 246 | } 247 | 248 | throw std::runtime_error ("unsupported distortion model"); 249 | }*/ 250 | -------------------------------------------------------------------------------- /src/conversion.hh: -------------------------------------------------------------------------------- 1 | #ifndef VISP_TRACKER_CONVERSION_HH 2 | # define VISP_TRACKER_CONVERSION_HH 3 | # include 4 | 5 | # include 6 | 7 | # include 8 | # include 9 | # include 10 | 11 | # include 12 | # include 13 | # include 14 | # include 15 | /* 16 | # include 17 | # include 18 | */ 19 | /// \brief Convert a ROS image into a ViSP one. 20 | /// 21 | /// This function copy a ROS image into a ViSP image. 22 | /// If the size are not matching, the ViSP image will be 23 | /// resized. 24 | /// 25 | /// \warning Some encodings only are supported. 26 | /// 27 | /// \param dst ViSP destination image 28 | /// \param src ROS source image 29 | void rosImageToVisp(vpImage& dst, 30 | const sensor_msgs::Image::ConstPtr& src); 31 | 32 | /// \brief Convert a ViSP image into a ROS one. 33 | /// 34 | /// This function copy a ViSP image into a ROS image. 35 | /// The whole content of the ROS image will be reset except 36 | /// the following field which will not be set: 37 | /// - header 38 | /// - is_bigendian 39 | /// 40 | /// \param dst ROS destination image 41 | /// \param src ViSP source image 42 | void vispImageToRos(sensor_msgs::Image& dst, 43 | const vpImage& src); 44 | 45 | /* 46 | void vpHomogeneousMatrixToTransform(geometry_msgs::Transform& dst, 47 | const vpHomogeneousMatrix& src); 48 | 49 | void transformToVpHomogeneousMatrix(vpHomogeneousMatrix& dst, 50 | const geometry_msgs::Transform& src); 51 | 52 | void convertMovingEdgeConfigToVpMe(const visp_tracker::MovingEdgeConfig& config, 53 | vpMe& moving_edge, 54 | vpMbEdgeTracker& tracker); 55 | 56 | void convertVpMeToMovingEdgeConfig(const vpMe& moving_edge, 57 | const vpMbEdgeTracker& tracker, 58 | visp_tracker::MovingEdgeConfig& config); 59 | 60 | void convertVpMeToInitRequest(const vpMe& moving_edge, 61 | const vpMbEdgeTracker& tracker, 62 | visp_tracker::Init& srv); 63 | 64 | void convertInitRequestToVpMe(const visp_tracker::Init::Request& req, 65 | vpMbEdgeTracker& tracker, 66 | vpMe& moving_edge); 67 | 68 | void initializeVpCameraFromCameraInfo(vpCameraParameters& cam, 69 | sensor_msgs::CameraInfoConstPtr info); 70 | */ 71 | #endif //! VISP_TRACKER_CONVERSION_HH 72 | -------------------------------------------------------------------------------- /src/homography_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | #include 23 | 24 | #include "conversion.hh" 25 | #include "callbacks.hh" 26 | 27 | 28 | void printMat(CvMat *A) 29 | { 30 | int i, j; 31 | for (i = 0; i < A->rows; i++) 32 | { 33 | printf("\n"); 34 | switch (CV_MAT_DEPTH(A->type)) 35 | { 36 | case CV_32F: 37 | case CV_64F: 38 | for (j = 0; j < A->cols; j++) 39 | printf ("%8.3f ", (float)cvGetReal2D(A, i, j)); 40 | break; 41 | case CV_8U: 42 | case CV_16U: 43 | for(j = 0; j < A->cols; j++) 44 | printf ("%6d",(int)cvGetReal2D(A, i, j)); 45 | break; 46 | default: 47 | break; 48 | } 49 | } 50 | printf("\n"); 51 | } 52 | 53 | int main(int argc, char **argv) 54 | { 55 | int N = 4; 56 | 57 | CvMat * src = cvCreateMat( N, 3, CV_64FC1); 58 | CvMat * srcT = cvCreateMat( 3, N, CV_64FC1); 59 | CvMat * dst = cvCreateMat( N, 3, CV_64FC1); 60 | CvMat * dstT = cvCreateMat( 3, N, CV_64FC1); 61 | 62 | CvMat * out = cvCreateMat( N, 3, CV_64FC1); 63 | CvMat * outT = cvCreateMat( 3, N, CV_64FC1); 64 | 65 | CvMat * homography_ = cvCreateMat(3, 3, CV_64FC1); 66 | CvMat * mask= cvCreateMat(1, N, CV_8UC1); 67 | 68 | 69 | src->data.db[0] = 0.0+1; 70 | src->data.db[1] = 0.0+1; 71 | src->data.db[2] = 1.0; 72 | 73 | src->data.db[3] = 1.0+1; 74 | src->data.db[4] = 0.0+1; 75 | src->data.db[5] = 1.0; 76 | 77 | src->data.db[6] = 1.0+1; 78 | src->data.db[7] = 1.0+1; 79 | src->data.db[8] = 1.0; 80 | 81 | src->data.db[9] = 0.0+1; 82 | src->data.db[10] = 1.0+1; 83 | src->data.db[11] = 1.0; 84 | 85 | cvTranspose(src, srcT); 86 | 87 | dst->data.db[0] = 0.0; 88 | dst->data.db[1] = 0.0; 89 | dst->data.db[2] = 1.0; 90 | 91 | dst->data.db[3] = 2.0; 92 | dst->data.db[4] = 0.0; 93 | dst->data.db[5] = 1.0; 94 | 95 | dst->data.db[6] = 2.0; 96 | dst->data.db[7] = 2.0; 97 | dst->data.db[8] = 1.0; 98 | 99 | dst->data.db[9] = 0.0; 100 | dst->data.db[10] = 2.0; 101 | dst->data.db[11] = 1.0; 102 | 103 | cvTranspose(dst, dstT); 104 | 105 | cvFindHomography(src, dst, homography_,0, 0, mask); 106 | 107 | printf("src"); 108 | printMat(src); 109 | printf("srcT"); 110 | printMat(srcT); 111 | printf("dst"); 112 | printMat(dst); 113 | printf("hom"); 114 | printMat(homography_); 115 | 116 | cvMatMul(homography_, srcT, outT); 117 | 118 | printf("outT"); 119 | printMat(outT); 120 | 121 | cvMatMul(src, homography_, out); 122 | 123 | printf("out"); 124 | printMat(out); 125 | 126 | } 127 | -------------------------------------------------------------------------------- /src/visp_dot_tracker.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | #include 23 | 24 | #include "conversion.hh" 25 | #include "callbacks.hh" 26 | 27 | /* 28 | // read 3D model from parameters 29 | XmlRpc::XmlRpcValue modelpoints__x_list; 30 | XmlRpc::XmlRpcValue modelpoints__y_list; 31 | XmlRpc::XmlRpcValue modelpoints__z_list; 32 | ros::param::get(visp_camera_calibration::modelpoints__x_param,modelpoints__x_list); 33 | ros::param::get(visp_camera_calibration::modelpoints__y_param,modelpoints__y_list); 34 | ros::param::get(visp_camera_calibration::modelpoints__z_param,modelpoints__z_list); 35 | ROS_ASSERT(modelpoints__x_list.getType() == XmlRpc::XmlRpcValue::TypeArray); 36 | ROS_ASSERT(modelpoints__x_list.size() == modelpoints__y_list.size() && modelpoints__x_list.size()==modelpoints__z_list.size()); 37 | for(int i=0;i(modelpoints__x_list[i]); 42 | double Y = static_cast(modelpoints__y_list[i]); 43 | double Z = static_cast(modelpoints__z_list[i]); 44 | vpPoint p; 45 | p.setWorldCoordinates(X,Y,Z); 46 | modelpoints__.push_back(p); 47 | } 48 | **/ 49 | 50 | 51 | class DotTracker 52 | { 53 | public: 54 | typedef vpImage image_t; 55 | typedef std::vector points_t; 56 | typedef std::vector imagePoints_t; 57 | 58 | DotTracker(unsigned queueSize = 5u); 59 | 60 | void spin(); 61 | void waitForImage(); 62 | void waitForInit(); 63 | void pointCorrespondenceCallback(const visp_camera_calibration::CalibPointArrayPtr& msg); 64 | 65 | unsigned queueSize_; 66 | 67 | ros::NodeHandle nodeHandle_; 68 | image_transport::ImageTransport imageTransport_; 69 | 70 | image_t image_; 71 | 72 | std::string rectifiedImageTopic_; 73 | std::string cameraInfoTopic_; 74 | std::string pointCorrespondenceTopic_ ; 75 | 76 | std::string model_prefix_; 77 | 78 | image_transport::CameraSubscriber cameraSubscriber_; 79 | ros::Subscriber pointCorrespondenceSubscriber_; 80 | 81 | std_msgs::Header header_; 82 | sensor_msgs::CameraInfoConstPtr info_; 83 | 84 | visp_camera_calibration::CalibPointArrayPtr pointMsg_; 85 | 86 | std::vector points_; 87 | std::vector > trackers_; 88 | 89 | CvMat * homography_; 90 | }; 91 | 92 | void printMat(CvMat *A) 93 | { 94 | int i, j; 95 | for (i = 0; i < A->rows; i++) 96 | { 97 | printf("\n"); 98 | switch (CV_MAT_DEPTH(A->type)) 99 | { 100 | case CV_32F: 101 | case CV_64F: 102 | for (j = 0; j < A->cols; j++) 103 | printf ("%8.3f ", (float)cvGetReal2D(A, i, j)); 104 | break; 105 | case CV_8U: 106 | case CV_16U: 107 | for(j = 0; j < A->cols; j++) 108 | printf ("%6d",(int)cvGetReal2D(A, i, j)); 109 | break; 110 | default: 111 | break; 112 | } 113 | } 114 | printf("\n"); 115 | } 116 | 117 | int main(int argc, char **argv) 118 | { 119 | ros::init(argc, argv, "dot_tracker"); 120 | try 121 | { 122 | DotTracker tracker(1); 123 | if (ros::ok()) 124 | tracker.spin(); 125 | ROS_INFO_STREAM("Finishing spinning"); 126 | } 127 | catch (std::exception& e) 128 | { 129 | std::cerr << "fatal error: " << e.what() << std::endl; 130 | ROS_ERROR_STREAM("fatal error: " << e.what()); 131 | return 1; 132 | } 133 | catch (...) 134 | { 135 | ROS_ERROR_STREAM("unexpected error"); 136 | return 2; 137 | } 138 | return 0; 139 | } 140 | 141 | 142 | DotTracker::DotTracker(unsigned queueSize) 143 | : queueSize_(queueSize), 144 | nodeHandle_(), 145 | imageTransport_(nodeHandle_), 146 | image_(), 147 | rectifiedImageTopic_(), 148 | cameraInfoTopic_(), 149 | pointCorrespondenceTopic_(), 150 | model_prefix_(), 151 | cameraSubscriber_(), 152 | pointCorrespondenceSubscriber_(), 153 | pointMsg_(), 154 | points_(), 155 | trackers_(), 156 | homography_() 157 | 158 | { 159 | 160 | homography_ = cvCreateMat(3, 3, CV_64FC1); 161 | // find camera prefix name 162 | ros::Rate rate (1); 163 | while (rectifiedImageTopic_.empty ()) 164 | { 165 | ros::param::get ("DotTracker/camera_topic", rectifiedImageTopic_); 166 | ros::param::get ("DotTracker/camera_info_topic", cameraInfoTopic_); 167 | ros::param::get ("DotTracker/point_correspondence_topic", pointCorrespondenceTopic_); 168 | ros::param::get ("DotTracker/model_prefix", model_prefix_); 169 | if (!ros::param::has ("DotTracker/camera_topic")) 170 | { 171 | ROS_WARN 172 | ("the camera_prefix parameter does not exist.\n" 173 | "This may mean that:\n" 174 | "- the tracker is not launched,\n" 175 | "- the tracker and viewer are not running in the same namespace." 176 | ); 177 | } 178 | else if (rectifiedImageTopic_.empty ()) 179 | { 180 | ROS_INFO 181 | ("tracker is not yet initialized, waiting...\n" 182 | "You may want to launch the client to initialize the tracker."); 183 | } 184 | if (!ros::ok ()) 185 | return; 186 | rate.sleep (); 187 | } 188 | 189 | ROS_INFO_STREAM("camera_topic is " << rectifiedImageTopic_); 190 | ROS_INFO_STREAM("camera_info_topic is " << cameraInfoTopic_); 191 | ROS_INFO_STREAM("point_correspondence_topic is " << pointCorrespondenceTopic_); 192 | 193 | 194 | cameraSubscriber_ = imageTransport_.subscribeCamera 195 | (rectifiedImageTopic_, queueSize_, 196 | bindImageCallback(image_, header_, info_)); 197 | //cameraSubscriber_ = imageTransport_.subscribe(rectifiedImageTopic_, queueSize_, imageCallback); 198 | // Wait for the image to be initialized. 199 | 200 | //register callback for pointCorrespondance 201 | pointCorrespondenceSubscriber_ = nodeHandle_.subscribe(pointCorrespondenceTopic_, 1, &DotTracker::pointCorrespondenceCallback, this); 202 | //nodeHandle_.subscribe(pointCorrespondenceTopic_, 1, pointCorrespondenceCallbackGlobal); 203 | 204 | 205 | waitForImage(); 206 | 207 | waitForInit(); 208 | } 209 | 210 | bool epsilonEquals(double a, double b, double e){ 211 | return (a + e > b) && (a - e < b); 212 | } 213 | 214 | void DotTracker::pointCorrespondenceCallback(const visp_camera_calibration::CalibPointArrayPtr& msg){ 215 | 216 | ROS_INFO_STREAM("pointCorrespondenceCallback"); 217 | points_.clear(); 218 | trackers_.clear(); 219 | 220 | 221 | std::string modelpoints__x_param(model_prefix_ + "/model_points_x"); 222 | std::string modelpoints__y_param(model_prefix_ + "/model_points_y"); 223 | std::string modelpoints__z_param(model_prefix_ + "/model_points_z"); 224 | 225 | XmlRpc::XmlRpcValue modelpoints__x_list; 226 | XmlRpc::XmlRpcValue modelpoints__y_list; 227 | XmlRpc::XmlRpcValue modelpoints__z_list; 228 | ros::param::get(modelpoints__x_param,modelpoints__x_list); 229 | ros::param::get(modelpoints__y_param,modelpoints__y_list); 230 | ros::param::get(modelpoints__z_param,modelpoints__z_list); 231 | 232 | ROS_INFO_STREAM(modelpoints__x_param << " value " << modelpoints__x_list); 233 | ROS_INFO_STREAM(modelpoints__y_param << " value " << modelpoints__y_list); 234 | ROS_INFO_STREAM(modelpoints__z_param << " value " << modelpoints__z_list); 235 | 236 | ROS_ASSERT(modelpoints__x_list.getType() == XmlRpc::XmlRpcValue::TypeArray); 237 | ROS_ASSERT(modelpoints__x_list.size() == modelpoints__y_list.size() && modelpoints__x_list.size()==modelpoints__z_list.size()); 238 | for(int i=0;iinitTracking(image_, vpImagePoint(element.i, element.j) , 10);//dodgy??? 281 | 282 | 283 | ROS_INFO_STREAM("found " << element2.i << " j=" << element2.j << "," << element2.X << "," << element2.Y << "," << element2.Z); 284 | } 285 | } 286 | } 287 | } 288 | 289 | 290 | void 291 | DotTracker::spin() 292 | { 293 | int count = 0; 294 | boost::format fmtWindowTitle ("ViSP Dot tracker - [ns: %s]"); 295 | fmtWindowTitle % ros::this_node::getNamespace (); 296 | 297 | vpDisplayX d(image_, image_.getWidth(), image_.getHeight(), 298 | fmtWindowTitle.str().c_str()); 299 | 300 | ros::Rate loop_rate_tracking(200); 301 | bool ok = false; 302 | vpHomogeneousMatrix cMo; 303 | vpImagePoint point (10, 10); 304 | while (!ok && ros::ok()) 305 | { 306 | 307 | try 308 | { 309 | //ROS_INFO_STREAM("spin once area"); 310 | vpDisplay::display(image_); 311 | 312 | for(uint i=0;itrack(image_); 315 | trackers_[i]->display(image_, vpColor::red); 316 | }catch(...){ 317 | //ROS_ERROR_STREAM("failed to track dot " << i); 318 | } 319 | 320 | } 321 | 322 | int N = points_.size(); 323 | 324 | double srcData[N*2]; 325 | double dstData[N*2]; 326 | CvMat * src = cvCreateMat( N, 3, CV_64FC1); 327 | CvMat * dst = cvCreateMat( N, 3, CV_64FC1); 328 | 329 | CvMat * mask= cvCreateMat(1, N, CV_8UC1); 330 | for(int i = 0; i < N; i++ ){ 331 | //model is src 332 | src->data.db[i*3] = points_[i].X; 333 | src->data.db[i*3+1] = points_[i].Y; 334 | src->data.db[i*3+2] = 1; 335 | 336 | //screen is dst 337 | dst->data.db[i*3] = trackers_[i]->getCog().get_i(); 338 | dst->data.db[i*3+1] = trackers_[i]->getCog().get_j(); 339 | dst->data.db[i*3+2] = 1; 340 | 341 | //ROS_INFO_STREAM("trackers_[i]->getCog() =" << trackers_[i]->getCog().get_i() << ", " << trackers_[i]->getCog().get_j()); 342 | //ROS_INFO_STREAM("points_[i] =" << points_[i].X << ", " << points_[i].Y); 343 | } 344 | 345 | 346 | 347 | 348 | cvFindHomography(src, dst, homography_, CV_LMEDS, 0, mask); 349 | 350 | if(count++ % 10 == 0){ 351 | count =0; 352 | printMat(homography_); 353 | } 354 | 355 | for(int i = 0; i < N; i++ ){ 356 | 357 | if(((int)(mask->data.ptr[i])) == 0){ 358 | //note we have to transpose 359 | CvMat * dst2 = cvCreateMat( 3, 1, CV_64FC1); //screen (unkown) 360 | CvMat * src2 = cvCreateMat( 3, 1, CV_64FC1); //model (known) 361 | 362 | src2->data.db[0] = points_[i].X; 363 | src2->data.db[1] = points_[i].Y; 364 | src2->data.db[2] = 1.0; 365 | 366 | cvMatMul(homography_, src2, dst2); 367 | 368 | dst2->data.db[0] /= dst2->data.db[2]; 369 | dst2->data.db[1] /= dst2->data.db[2]; 370 | dst2->data.db[2] /= dst2->data.db[2]; 371 | 372 | //ROS_INFO_STREAM("trackers_[i]->getCog() =" << trackers_[i]->getCog().get_i() << ", " << trackers_[i]->getCog().get_j()); 373 | //ROS_INFO_STREAM("for point number: " << i << " model x = " << points_[i].X << " model y = " << points_[i].Y); 374 | //ROS_INFO_STREAM("setting tracker "<< i << " to x = "<< dst2->data.db[0] << ", y = " << dst2->data.db[1] << "," << dst2->data.db[2]); 375 | 376 | try{ 377 | //trackers_[i]->initTracking(image_, vpImagePoint(dst2->data.db[0], dst2->data.db[1]) , 15);//dodgy??? 378 | //trackers_[i]->getCog().set_i(dst2->data.db[0]); 379 | //trackers_[i]->getCog().set_j(dst2->data.db[1]); 380 | 381 | //ROS_INFO_STREAM("setting tracker "<< i << " to x = "<< dst2->data.db[0] << ", y = " << dst2->data.db[1] << "," << dst2->data.db[2]); 382 | 383 | boost::shared_ptr tracker(new vpDot2()); 384 | 385 | tracker->setSizePrecision(.1); 386 | tracker->setEllipsoidShapePrecision(.1); 387 | tracker->setGrayLevelPrecision(.65); 388 | 389 | trackers_[i] = tracker; 390 | trackers_[i]->initTracking(image_, vpImagePoint(dst2->data.db[0], dst2->data.db[1]) , 15); 391 | 392 | trackers_[i]->track(image_); 393 | trackers_[i]->display(image_, vpColor::green); 394 | }catch(...){ 395 | ROS_ERROR_STREAM("failed to track dot " << i); 396 | } 397 | 398 | //trackers_[i]->getCog().set_i(dst2->data.db[0]); 399 | //trackers_[i]->getCog().set_j(dst2->data.db[1]); 400 | } 401 | } 402 | /* 403 | ROS_INFO_STREAM("mask="); 404 | for(int i = 0; i < N; i++ ){ 405 | ROS_INFO_STREAM((int)mask->data.ptr[i]<<" "); 406 | } 407 | ROS_INFO("\n"); 408 | for(int i = 0; i < 3; i++ ){ 409 | for(int j = 0; j < 3; j++ ){ 410 | 411 | ROS_INFO_STREAM(hom_ret->data.db[i + j*3]<<" "); 412 | } 413 | ROS_INFO("\n"); 414 | } 415 | ROS_INFO("\n"); 416 | */ 417 | vpDisplay::flush(image_); 418 | 419 | ros::spinOnce(); 420 | loop_rate_tracking.sleep(); 421 | } 422 | catch(const std::runtime_error& e) 423 | { 424 | ROS_ERROR_STREAM("C failed to initialize: " 425 | << e.what() << ", retrying..."); 426 | } 427 | catch(const std::string& str) 428 | { 429 | ROS_ERROR_STREAM("B failed to initialize: " 430 | << str << ", retrying..."); 431 | } 432 | catch(...) 433 | { 434 | ROS_ERROR("A failed to initialize, retrying..."); 435 | } 436 | } 437 | } 438 | 439 | void 440 | DotTracker::waitForImage() 441 | { 442 | ros::Rate loop_rate(10); 443 | while (ros::ok() 444 | && (!image_.getWidth() || !image_.getHeight())) 445 | { 446 | ROS_INFO_THROTTLE(1, "waiting for an image..."); 447 | ros::spinOnce(); 448 | loop_rate.sleep(); 449 | } 450 | } 451 | 452 | void 453 | DotTracker::waitForInit() 454 | { 455 | ros::Rate loop_rate(10); 456 | while (ros::ok() 457 | && (points_.size() == 0)) 458 | { 459 | ROS_INFO_THROTTLE(1, "waiting for initial points ..."); 460 | ros::spinOnce(); 461 | loop_rate.sleep(); 462 | } 463 | } 464 | -------------------------------------------------------------------------------- /todo.txt: -------------------------------------------------------------------------------- 1 | 2 | --------------------------------------------------------------------------------