| @@ -0,0 +1,49 @@ | ||
| 0.5, 0, 0, 0, 0, 0, 0 | ||
| 1, 1, 4, 0, 3, 1, 2 | ||
| 1.5, 1, 12, -3, 13, -2, 9 | ||
| 2, 1, 3, 3, 2, 1, 3 | ||
| 2.5, 1, 4, -1, 5, 1, 3 | ||
| 3, -156, -133, 7.37789e+08, -32810, -5.49648e+07, 283 | ||
| 3.5, 0, 1, 1, 2, 1, 3 | ||
| 4, 2, 4, -3, 6, -1, 3 | ||
| 4.5, 2, 5, 0, 4, 2, 3 | ||
| 5, 1, 2, 2, 1, 0, 2 | ||
| 5.5, -2, 7, -2, 7, -2, 4 | ||
| 6, 2, 1, -2, 2, -2, -1 | ||
| 6.5, -2, 0, -4, -3, -3, -4 | ||
| 7, 2, 3, -1, 3, 1, 1 | ||
| 7.5, -3, 13, -12, 20, -16, 2 | ||
| 8, -12, 13, -20, 10, -17, 5 | ||
| 8.5, 1, 4, -1, 2, 0, 1 | ||
| 9, 2, 1, -1, -1, 0, -1 | ||
| 9.5, 1, 2, -2, 1, -3, -1 | ||
| 10, 0, 3, -2, 1, -2, -2 | ||
| 10.5, -5, 0, -7, 1, -7, -1 | ||
| 11, -9, -7, -10, -2, -8, -6 | ||
| 11.5, -12, -4, -10, -6, -10, -4 | ||
| 12, 15, 12, -20, 4, 10, 19 | ||
| 12.5, -5, 1, -5, 0, -5, 0 | ||
| 13, -7, 1, -8, 2, -9, -1 | ||
| 13.5, -3, -2, -3, 0, -4, 0 | ||
| 14, -6, 1, -10, 2, -8, -1 | ||
| 14.5, -3, 0, -4, 1, -6, 1 | ||
| 15, -2, 0, -1, 1, -3, 2 | ||
| 15.5, -3, -1, 7, -4, -34, -34 | ||
| 16, -60, -177, -76, -144, -98, -147 | ||
| 16.5, -72, -65, -73, -41, -107, -56 | ||
| 17, 1, 3, 4, 4, 8, 8 | ||
| 17.5, -3, -1, -3, 0, -2, 0 | ||
| 18, 0, 0, 1, 0, 0, 0 | ||
| 18.5, 0, 0, 0, 0, 0, -1 | ||
| 19, 0, -1, 0, -1, -2, 0 | ||
| 19.5, -1, 0, -1, 0, 1, 1 | ||
| 20, 0, 0, -1, 1, 0, 0 | ||
| 20.5, -1, 2, -3, 0, 0, 0 | ||
| 21, -4, 6, -9, 4, -10, 2 | ||
| 21.5, -2, 5, -7, 4, -6, 0 | ||
| 22, -3, 7, -8, -1, -12, 1 | ||
| 22.5, -5, 1, -7, 1, -9, -2 | ||
| 23, -7, 3, -12, 3, -12, -2 | ||
| 23.5, -6, 1, -11, 0, -11, -4 | ||
| 24, -7, 1, -12, 0, -12, -3 | ||
| 24.5, -12, 2, -17, 1, -20, -5 |
| @@ -0,0 +1,8 @@ | ||
| This folder contain the plugin for the combined part of the RoVi final project | ||
|
|
||
| How to compile the project: | ||
| 1) Create build folder | ||
| 2) Open build folder in cmd | ||
| 3) Write "cmake .." in cmd | ||
| 4) Write "make" in cmd | ||
| 5) The plugin can now be found in the libs folder |
| @@ -0,0 +1,50 @@ | ||
| -0.0315 -0.819 1.649 0 0 -1.571 | ||
| -0.06649999999999999 -0.819 1.649 0 0 -1.571 | ||
| -0.10149999999999999 -0.819 1.649 0 0 -1.571 | ||
| -0.13649999999999998 -0.819 1.649 0 0 -1.571 | ||
| -0.17149999999999999 -0.819 1.649 0 0 -1.571 | ||
| -0.2065 -0.819 1.649 0 0 -1.571 | ||
| -0.24149999999999996 -0.819 1.649 0 0 -1.571 | ||
| -0.27649999999999997 -0.819 1.649 0 0 -1.571 | ||
| -0.3115 -0.819 1.649 0 0 -1.571 | ||
| -0.3465 -0.819 1.649 0 0 -1.571 | ||
| -0.35 -0.819 1.649 0 0.09 -1.571 | ||
| -0.35 -0.819 1.649 0 0.19 -1.571 | ||
| -0.35 -0.819 1.649 0 0.29 -1.571 | ||
| -0.35 -0.819 1.649 0 0.39 -1.571 | ||
| -0.35 -0.819 1.649 0 0.49 -1.571 | ||
| -0.35 -0.819 1.649 0 0.59 -1.571 | ||
| -0.35 -0.819 1.649 0 0.69 -1.571 | ||
| -0.35 -0.819 1.649 0 0.79 -1.571 | ||
| -0.35 -0.819 1.649 0 0.89 -1.571 | ||
| -0.35 -0.819 1.649 0 0.99 -1.571 | ||
| -0.3185 -0.819 1.6625 0 1 -1.571 | ||
| -0.2835 -0.819 1.6775 0 1 -1.571 | ||
| -0.2485 -0.819 1.6925 0 1 -1.571 | ||
| -0.2135 -0.819 1.7075 0 1 -1.571 | ||
| -0.1785 -0.819 1.7225 0 1 -1.571 | ||
| -0.1435 -0.819 1.7375 0 1 -1.571 | ||
| -0.10850000000000001 -0.819 1.7525 0 1 -1.571 | ||
| -0.07350000000000001 -0.819 1.7675 0 1 -1.571 | ||
| -0.03849999999999998 -0.819 1.7825 0 1 -1.571 | ||
| -0.003500000000000003 -0.819 1.7974999999999999 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| -0.0315 -0.819 1.799 0 1.09 -1.571 | ||
| -0.06649999999999999 -0.819 1.799 0 1.19 -1.571 | ||
| -0.10149999999999999 -0.819 1.799 0 1.29 -1.571 | ||
| -0.13649999999999998 -0.819 1.799 0 1.39 -1.571 | ||
| -0.17149999999999999 -0.819 1.799 0 1.49 -1.571 | ||
| -0.2065 -0.819 1.799 0 1.59 -1.571 | ||
| -0.24149999999999996 -0.819 1.799 0 1.69 -1.571 | ||
| -0.27649999999999997 -0.819 1.799 0 1.79 -1.571 | ||
| -0.3115 -0.819 1.799 0 1.89 -1.571 | ||
| -0.3465 -0.819 1.799 0 1.99 -1.571 |
| @@ -0,0 +1,167 @@ | ||
| -0.006999999999999999 -0.819 1.649 0 0 -1.571 | ||
| -0.017499999999999998 -0.819 1.649 0 0 -1.571 | ||
| -0.027999999999999997 -0.819 1.649 0 0 -1.571 | ||
| -0.0385 -0.819 1.649 0 0 -1.571 | ||
| -0.049 -0.819 1.649 0 0 -1.571 | ||
| -0.0595 -0.819 1.649 0 0 -1.571 | ||
| -0.06999999999999999 -0.819 1.649 0 0 -1.571 | ||
| -0.0805 -0.819 1.649 0 0 -1.571 | ||
| -0.091 -0.819 1.649 0 0 -1.571 | ||
| -0.10149999999999999 -0.819 1.649 0 0 -1.571 | ||
| -0.11199999999999999 -0.819 1.649 0 0 -1.571 | ||
| -0.12249999999999998 -0.819 1.649 0 0 -1.571 | ||
| -0.13299999999999998 -0.819 1.649 0 0 -1.571 | ||
| -0.1435 -0.819 1.649 0 0 -1.571 | ||
| -0.154 -0.819 1.649 0 0 -1.571 | ||
| -0.16449999999999998 -0.819 1.649 0 0 -1.571 | ||
| -0.175 -0.819 1.649 0 0 -1.571 | ||
| -0.1855 -0.819 1.649 0 0 -1.571 | ||
| -0.196 -0.819 1.649 0 0 -1.571 | ||
| -0.2065 -0.819 1.649 0 0 -1.571 | ||
| -0.217 -0.819 1.649 0 0 -1.571 | ||
| -0.22749999999999998 -0.819 1.649 0 0 -1.571 | ||
| -0.238 -0.819 1.649 0 0 -1.571 | ||
| -0.24849999999999997 -0.819 1.649 0 0 -1.571 | ||
| -0.259 -0.819 1.649 0 0 -1.571 | ||
| -0.26949999999999996 -0.819 1.649 0 0 -1.571 | ||
| -0.27999999999999997 -0.819 1.649 0 0 -1.571 | ||
| -0.2905 -0.819 1.649 0 0 -1.571 | ||
| -0.301 -0.819 1.649 0 0 -1.571 | ||
| -0.3115 -0.819 1.649 0 0 -1.571 | ||
| -0.322 -0.819 1.649 0 0 -1.571 | ||
| -0.33249999999999996 -0.819 1.649 0 0 -1.571 | ||
| -0.34299999999999997 -0.819 1.649 0 0 -1.571 | ||
| -0.35 -0.819 1.649 0 0.01 -1.571 | ||
| -0.35 -0.819 1.649 0 0.04 -1.571 | ||
| -0.35 -0.819 1.649 0 0.07 -1.571 | ||
| -0.35 -0.819 1.649 0 0.1 -1.571 | ||
| -0.35 -0.819 1.649 0 0.13 -1.571 | ||
| -0.35 -0.819 1.649 0 0.16 -1.571 | ||
| -0.35 -0.819 1.649 0 0.19 -1.571 | ||
| -0.35 -0.819 1.649 0 0.22 -1.571 | ||
| -0.35 -0.819 1.649 0 0.25 -1.571 | ||
| -0.35 -0.819 1.649 0 0.28 -1.571 | ||
| -0.35 -0.819 1.649 0 0.31 -1.571 | ||
| -0.35 -0.819 1.649 0 0.34 -1.571 | ||
| -0.35 -0.819 1.649 0 0.37 -1.571 | ||
| -0.35 -0.819 1.649 0 0.40 -1.571 | ||
| -0.35 -0.819 1.649 0 0.43 -1.571 | ||
| -0.35 -0.819 1.649 0 0.46 -1.571 | ||
| -0.35 -0.819 1.649 0 0.49 -1.571 | ||
| -0.35 -0.819 1.649 0 0.52 -1.571 | ||
| -0.35 -0.819 1.649 0 0.55 -1.571 | ||
| -0.35 -0.819 1.649 0 0.58 -1.571 | ||
| -0.35 -0.819 1.649 0 0.61 -1.571 | ||
| -0.35 -0.819 1.649 0 0.64 -1.571 | ||
| -0.35 -0.819 1.649 0 0.67 -1.571 | ||
| -0.35 -0.819 1.649 0 0.70 -1.571 | ||
| -0.35 -0.819 1.649 0 0.73 -1.571 | ||
| -0.35 -0.819 1.649 0 0.76 -1.571 | ||
| -0.35 -0.819 1.649 0 0.79 -1.571 | ||
| -0.35 -0.819 1.649 0 0.82 -1.571 | ||
| -0.35 -0.819 1.649 0 0.85 -1.571 | ||
| -0.35 -0.819 1.649 0 0.88 -1.571 | ||
| -0.35 -0.819 1.649 0 0.91 -1.571 | ||
| -0.35 -0.819 1.649 0 0.94 -1.571 | ||
| -0.35 -0.819 1.649 0 0.97 -1.571 | ||
| -0.35 -0.819 1.649 0 1 -1.571 | ||
| -0.33949999999999997 -0.819 1.6535 0 1 -1.571 | ||
| -0.32899999999999996 -0.819 1.658 0 1 -1.571 | ||
| -0.3185 -0.819 1.6625 0 1 -1.571 | ||
| -0.308 -0.819 1.667 0 1 -1.571 | ||
| -0.2975 -0.819 1.6715 0 1 -1.571 | ||
| -0.287 -0.819 1.676 0 1 -1.571 | ||
| -0.27649999999999997 -0.819 1.6805 0 1 -1.571 | ||
| -0.266 -0.819 1.685 0 1 -1.571 | ||
| -0.25549999999999995 -0.819 1.6895 0 1 -1.571 | ||
| -0.245 -0.819 1.694 0 1 -1.571 | ||
| -0.2345 -0.819 1.6985 0 1 -1.571 | ||
| -0.22399999999999998 -0.819 1.703 0 1 -1.571 | ||
| -0.2135 -0.819 1.7075 0 1 -1.571 | ||
| -0.20299999999999999 -0.819 1.712 0 1 -1.571 | ||
| -0.19249999999999998 -0.819 1.7165 0 1 -1.571 | ||
| -0.182 -0.819 1.721 0 1 -1.571 | ||
| -0.17149999999999999 -0.819 1.7255 0 1 -1.571 | ||
| -0.16099999999999998 -0.819 1.73 0 1 -1.571 | ||
| -0.1505 -0.819 1.7345 0 1 -1.571 | ||
| -0.13999999999999999 -0.819 1.7389999999999999 0 1 -1.571 | ||
| -0.1295 -0.819 1.7435 0 1 -1.571 | ||
| -0.119 -0.819 1.748 0 1 -1.571 | ||
| -0.10850000000000001 -0.819 1.7525 0 1 -1.571 | ||
| -0.09799999999999998 -0.819 1.757 0 1 -1.571 | ||
| -0.08750000000000002 -0.819 1.7614999999999998 0 1 -1.571 | ||
| -0.07700000000000001 -0.819 1.766 0 1 -1.571 | ||
| -0.0665 -0.819 1.7705 0 1 -1.571 | ||
| -0.055999999999999994 -0.819 1.775 0 1 -1.571 | ||
| -0.045499999999999985 -0.819 1.7794999999999999 0 1 -1.571 | ||
| -0.034999999999999976 -0.819 1.784 0 1 -1.571 | ||
| -0.024499999999999966 -0.819 1.7885 0 1 -1.571 | ||
| -0.014000000000000012 -0.819 1.793 0 1 -1.571 | ||
| -0.003500000000000003 -0.819 1.7974999999999999 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| 0 -0.819 1.799 0 1 -1.571 | ||
| -0.0034999999999999996 -0.819 1.799 0 1.01 -1.571 | ||
| -0.013999999999999999 -0.819 1.799 0 1.04 -1.571 | ||
| -0.0245 -0.819 1.799 0 1.07 -1.571 | ||
| -0.034999999999999996 -0.819 1.799 0 1.1 -1.571 | ||
| -0.0455 -0.819 1.799 0 1.13 -1.571 | ||
| -0.055999999999999994 -0.819 1.799 0 1.16 -1.571 | ||
| -0.06649999999999999 -0.819 1.799 0 1.19 -1.571 | ||
| -0.077 -0.819 1.799 0 1.22 -1.571 | ||
| -0.0875 -0.819 1.799 0 1.25 -1.571 | ||
| -0.098 -0.819 1.799 0 1.28 -1.571 | ||
| -0.1085 -0.819 1.799 0 1.31 -1.571 | ||
| -0.119 -0.819 1.799 0 1.34 -1.571 | ||
| -0.1295 -0.819 1.799 0 1.37 -1.571 | ||
| -0.13999999999999999 -0.819 1.799 0 1.4 -1.571 | ||
| -0.1505 -0.819 1.799 0 1.43 -1.571 | ||
| -0.161 -0.819 1.799 0 1.46 -1.571 | ||
| -0.17149999999999999 -0.819 1.799 0 1.49 -1.571 | ||
| -0.182 -0.819 1.799 0 1.52 -1.571 | ||
| -0.1925 -0.819 1.799 0 1.55 -1.571 | ||
| -0.20299999999999999 -0.819 1.799 0 1.58 -1.571 | ||
| -0.2135 -0.819 1.799 0 1.61 -1.571 | ||
| -0.22399999999999998 -0.819 1.799 0 1.64 -1.571 | ||
| -0.2345 -0.819 1.799 0 1.67 -1.571 | ||
| -0.24499999999999997 -0.819 1.799 0 1.70 -1.571 | ||
| -0.2555 -0.819 1.799 0 1.73 -1.571 | ||
| -0.26599999999999996 -0.819 1.799 0 1.76 -1.571 | ||
| -0.27649999999999997 -0.819 1.799 0 1.79 -1.571 | ||
| -0.287 -0.819 1.799 0 1.82 -1.571 | ||
| -0.2975 -0.819 1.799 0 1.85 -1.571 | ||
| -0.308 -0.819 1.799 0 1.88 -1.571 | ||
| -0.3185 -0.819 1.799 0 1.91 -1.571 | ||
| -0.32899999999999996 -0.819 1.799 0 1.84 -1.571 | ||
| -0.33949999999999997 -0.819 1.799 0 1.97 -1.571 | ||
| -0.35 -0.819 1.799 0 2 -1.571 |
| @@ -0,0 +1,109 @@ | ||
| #ifndef VISUALSERVOING_HPP | ||
| #define VISUALSERVOING_HPP | ||
|
|
||
| // RobWork includes | ||
| #include <rw/models/WorkCell.hpp> | ||
| #include <rw/kinematics/State.hpp> | ||
| #include <rwlibs/opengl/RenderImage.hpp> | ||
| #include <rwlibs/simulation/GLFrameGrabber.hpp> | ||
| #include <rw/loaders/ImageLoader.hpp> | ||
| #include <rw/loaders/WorkCellFactory.hpp> | ||
|
|
||
| // RobWorkStudio includes | ||
| #include <RobWorkStudioConfig.hpp> // For RWS_USE_QT5 definition | ||
| #include <rws/RobWorkStudioPlugin.hpp> | ||
| #include <rws/RobWorkStudio.hpp> | ||
|
|
||
| //OpenCV | ||
| #include <opencv2/opencv.hpp> | ||
| #include "opencv2/xfeatures2d.hpp" | ||
|
|
||
| //QT | ||
| #include <QTimer> | ||
| #include <QCheckBox> | ||
| #include <QLabel> | ||
| #include <QPushButton> | ||
| #include <QToolButton> | ||
| #include <QScrollArea> | ||
| #include <QDoubleSpinBox> | ||
|
|
||
| #include <fstream> | ||
|
|
||
|
|
||
| class VisualServoing: public rws::RobWorkStudioPlugin | ||
| { | ||
|
|
||
| Q_OBJECT | ||
| Q_INTERFACES( rws::RobWorkStudioPlugin ) | ||
| #if RWS_USE_QT5 | ||
| Q_PLUGIN_METADATA(IID "dk.sdu.mip.Robwork.RobWorkStudioPlugin/0.1" FILE "VisualServoing.json") | ||
| #endif | ||
|
|
||
| public: | ||
| VisualServoing(); | ||
| virtual ~VisualServoing(); | ||
|
|
||
| virtual void open(rw::models::WorkCell* workcell); | ||
|
|
||
| virtual void close(); | ||
|
|
||
| virtual void initialize(); | ||
|
|
||
|
|
||
|
|
||
| private: | ||
| QWidget* createMarkerButtons(); | ||
|
|
||
|
|
||
| rw::kinematics::State _state; | ||
| rw::models::WorkCell* _workcell; | ||
|
|
||
| QTimer* _timer; | ||
| QCheckBox* _cBox; | ||
| QWidget* _markerButtons; | ||
| QLabel* _camPicture; | ||
| QLabel* _processedPicture; | ||
| QPushButton* _initButton; | ||
| QPushButton* _background; | ||
| QPushButton* _motionBtn; | ||
| QDoubleSpinBox* _deltaTSpinBox; | ||
| cv::Mat _img_object; | ||
| std::vector<cv::KeyPoint> _keypoints_object; | ||
| cv::Mat _descriptors_object; | ||
| cv::Ptr<cv::xfeatures2d::SURF> _detector; | ||
| rwlibs::opengl::RenderImage *_textureRender, *_bgRender; | ||
| rwlibs::simulation::GLFrameGrabber* _framegrabber; | ||
|
|
||
| rw::kinematics::Frame* _markerFrame; | ||
| rw::kinematics::Frame* _cameraFrame; | ||
| rw::kinematics::Frame* _base; | ||
| rw::models::Device::Ptr _device; | ||
|
|
||
| std::ofstream trackingError; | ||
| std::ofstream robotq; | ||
| std::ofstream robotdq; | ||
|
|
||
| std::vector<std::vector<double>> _UV; | ||
| std::vector<double> _UVs; | ||
| std::vector<rw::math::Transform3D<>> transformsForMarker; | ||
| std::string motionFile = "/home/student/workspace/RoVi-2017/pluginV1/motions/MarkerMotionFast.txt"; | ||
| int transIterator; | ||
| int markerInUse; | ||
| double deltaT = 0.5; | ||
|
|
||
| private slots: | ||
| void stateChangedListener(const rw::kinematics::State& state); | ||
| void capture(); | ||
| void init(); | ||
| void loadMarker1(); | ||
| void loadMarker2(); | ||
| void loadMarker3(); | ||
| void loadBackground(); | ||
| void loadMotion(); | ||
| void nextMarkerPos(); | ||
| void nextMarkerPosMult(); | ||
| void deltaTChanged(double val); | ||
| void startSequence(int state); | ||
| }; | ||
|
|
||
| #endif |
| @@ -0,0 +1,5 @@ | ||
| { | ||
| "name" : "VisualServoingApp", | ||
| "version" : "1.0.0", | ||
| "dependencies" : [] | ||
| } |
| @@ -0,0 +1,340 @@ | ||
| #include "ip.h" | ||
|
|
||
|
|
||
| cv::Mat ip::segmentateHSV(cv::Mat image, int hueMin, int hueMax, int saturationMin, int saturationMax, int valueMin, int valueMax){ | ||
|
|
||
| //convert to HSV | ||
| cv::Mat HSV(image.rows, image.cols, CV_8UC3); | ||
| cv::cvtColor(image, HSV, CV_BGR2HSV); | ||
|
|
||
| cv::Mat dst; | ||
|
|
||
| inRange(HSV, cv::Scalar(hueMin,saturationMin,valueMin), cv::Scalar(hueMax,saturationMax,valueMax), dst); | ||
|
|
||
| return dst; | ||
| } | ||
|
|
||
| cv::Mat ip::opening(cv::Mat image, int elementType, int kernelSize){ | ||
| //elementType 0 = MORPTH_RECT, 1 = MORPH_CROSS, 2 = MORPH_ELLIPSE | ||
| cv::Mat dst; | ||
| cv::Mat element = cv::getStructuringElement(elementType, | ||
| cv::Size(2*kernelSize+1, 2*kernelSize+1), | ||
| cv::Point(kernelSize,kernelSize)); | ||
|
|
||
| cv::erode(image,dst,element); | ||
| cv::dilate(dst, dst, element); | ||
|
|
||
|
|
||
| return dst; | ||
| } | ||
|
|
||
| cv::Mat ip::closing(cv::Mat image, int elementType, int kernelSize){ | ||
| //elementType 0 = MORPTH_RECT, 1 = MORPH_CROSS, 2 = MORPH_ELLIPSE | ||
| cv::Mat dst; | ||
| cv::Mat element = cv::getStructuringElement(elementType, | ||
| cv::Size(2*kernelSize+1, 2*kernelSize+1), | ||
| cv::Point(kernelSize,kernelSize)); | ||
|
|
||
| cv::dilate(image,dst,element); | ||
| cv::erode(dst,dst,element); | ||
|
|
||
|
|
||
| return dst; | ||
| } | ||
|
|
||
| std::vector<std::vector<cv::Point> > ip::findContours(cv::Mat image){ | ||
|
|
||
| std::vector<std::vector<cv::Point> > contours; | ||
| cv::findContours(image,contours,cv::RETR_TREE, cv::CHAIN_APPROX_NONE ); | ||
|
|
||
| return contours; | ||
| } | ||
|
|
||
|
|
||
| cv::Mat ip::drawContours(std::vector<std::vector<cv::Point> > contours, cv::Mat image){ | ||
|
|
||
| cv::Mat contourImg(image.size(), CV_8UC3, cv::Scalar(0,0,0)); | ||
| cv::Scalar colors[3]; | ||
| colors[0] = cv::Scalar(255,0,0); | ||
| colors[1] = cv::Scalar(0,255,0); | ||
| colors[2] = cv::Scalar(0,0,255); | ||
|
|
||
| for (size_t i = 0; i < contours.size(); i++) { | ||
| cv::drawContours(contourImg, contours, i, colors[i % 3]); | ||
| } | ||
|
|
||
| return contourImg; | ||
| } | ||
|
|
||
| std::vector<cv::Moments> ip::getMu(std::vector<std::vector<cv::Point> > contours ){ | ||
| std::vector<cv::Moments> mu(contours.size() ); | ||
| for (int i = 0; i < contours.size(); i++) { | ||
| mu[i] = cv::moments(contours[i],false); | ||
| } | ||
| return mu; | ||
| } | ||
|
|
||
| std::vector<cv::Point2i> ip::getCenterPoints(std::vector<cv::Moments> moments, std::vector<std::vector<cv::Point> > contours){ | ||
| std::vector<cv::Point2i> centerPoints(contours.size()); | ||
| for (int i = 0; i < contours.size(); i++) { | ||
| centerPoints[i] = cv::Point2f(moments[i].m10/moments[i].m00, moments[i].m01/moments[i].m00); | ||
| } | ||
| return centerPoints; | ||
| } | ||
|
|
||
|
|
||
| cv::Mat ip::drawPoints(std::vector<cv::Point2i> points, cv::Mat image, cv::Vec3b color){ | ||
|
|
||
| cv::Mat dst = image.clone(); | ||
|
|
||
| for (int i = 0; i < points.size(); i++) { | ||
| dst.at<cv::Vec3b>(points[i]) = color; | ||
| } | ||
|
|
||
| return dst; | ||
| } | ||
|
|
||
| std::vector<cv::Point2i> ip::toRobotPoints(std::vector<cv::Point2i> points, cv::Mat image){ | ||
|
|
||
| cv::Point2i UV((image.cols)/2,(image.rows)/2); | ||
| std::vector<cv::Point2i> newPoints = points; | ||
| //std::cout << "x: " << UV.x << " y: " << UV.y << std::endl; | ||
| for (int i = 0; i < points.size() ; i++) { | ||
| // std::cout << "x: " << (UV-points[i]).x << " y: " << (UV-points[i]).y << std::endl; | ||
| newPoints[i] = points[i]-UV; | ||
| //newPoints[i].x = -newPoints[i].x; | ||
| //newPoints[i].y = -newPoints[i].y; | ||
| } | ||
| return newPoints; | ||
| } | ||
|
|
||
| std::vector<cv::Point2i> ip::decideOnBlueMarkers(std::vector<cv::Point2i> mcBlue, std::vector<cv::Point2i> mcRed) { | ||
|
|
||
| if(mcRed.size() < 1 || mcBlue.size() < 3) | ||
| return mcRed; | ||
|
|
||
| cv::Vec2i vector1 = {mcBlue[0].x - mcRed[0].x,mcBlue[0].y - mcRed[0].y}; | ||
| cv::Vec2i vector2 = {mcBlue[1].x - mcRed[0].x,mcBlue[1].y - mcRed[0].y}; | ||
| cv::Vec2i vector3 = {mcBlue[2].x - mcRed[0].x,mcBlue[2].y - mcRed[0].y}; | ||
|
|
||
| std::vector<cv::Point2i> points; | ||
|
|
||
| std::vector<int> dotProducts = {vector1.dot(vector2), vector1.dot(vector3),vector2.dot(vector3)}; | ||
| int idx = std::min_element(dotProducts.begin(),dotProducts.end())-dotProducts.begin(); | ||
| if(idx == 0){ | ||
|
|
||
| int crossProduct = vector1[0]*vector2[1] - vector1[1]*vector2[0];; | ||
| if(crossProduct < 0){ | ||
| points = {mcBlue[0],mcBlue[1],mcBlue[2]}; | ||
| } | ||
| else{ | ||
| points = {mcBlue[1],mcBlue[0],mcBlue[2]}; | ||
| } | ||
| } | ||
| else if(idx == 1){ | ||
| int crossProduct = vector1[0]*vector3[1] - vector1[1]*vector3[0]; | ||
| if(crossProduct < 0){ | ||
| points = {mcBlue[0],mcBlue[2],mcBlue[1]}; | ||
| } | ||
| else{ | ||
| points = {mcBlue[2],mcBlue[0],mcBlue[1]}; | ||
| } | ||
| } | ||
| else if(idx == 2){ | ||
| int crossProduct = vector2[0]*vector3[1] - vector2[1]*vector3[0]; | ||
| if(crossProduct < 0){ | ||
| points = {mcBlue[1],mcBlue[2],mcBlue[0]}; | ||
| } | ||
| else{ | ||
| points = {mcBlue[2],mcBlue[1],mcBlue[0]}; | ||
| } | ||
| } | ||
| return points; | ||
| } | ||
|
|
||
| cv::Mat ip::toOpenCVImage(const rw::sensor::Image& img) { | ||
| cv::Mat res(img.getHeight(),img.getWidth(), CV_8UC3); | ||
| res.data = (uchar*)img.getImageData(); | ||
| return res; | ||
| } | ||
|
|
||
| std::vector<cv::Point2i> ip::marker1Function(const rw::sensor::Image& image) { | ||
| // Convert to OpenCV image | ||
| cv::Mat im = toOpenCVImage(image); | ||
| cv::Mat imflip; | ||
| cv::flip(im, imflip, 0); | ||
| //imflip = im; | ||
| cv::cvtColor(imflip, imflip, CV_RGB2BGR); | ||
|
|
||
| cv::Mat segmentedBlue, segmentedRed; | ||
|
|
||
| //FIND BLUE | ||
| segmentedBlue = segmentateHSV(imflip, 120, 121, 250, 256, 100, 256); //blue circles | ||
| segmentedBlue = opening(segmentedBlue, 0, 3); | ||
| segmentedBlue = closing(segmentedBlue, 0, 6); | ||
| segmentedBlue = opening(segmentedBlue, 0, 14); | ||
|
|
||
| std::vector<std::vector<cv::Point> > contoursBlue = findContours(segmentedBlue); | ||
|
|
||
| //segmentedBlue = ip::drawContours(contoursBlue, segmentedBlue); | ||
|
|
||
| //Get the moments | ||
| std::vector<cv::Moments> muBlue = getMu(contoursBlue); | ||
|
|
||
| //get the mass centers | ||
| std::vector<cv::Point2i> mcBlue = getCenterPoints(muBlue, contoursBlue); | ||
|
|
||
| //FIND RED | ||
| segmentedRed = segmentateHSV(imflip, 0, 1, 250, 256, 100, 256); //blue circles | ||
| segmentedRed = opening(segmentedRed, 0, 3); | ||
| segmentedRed = closing(segmentedRed, 0, 6); | ||
| segmentedRed = opening(segmentedRed, 0, 14); | ||
|
|
||
| std::vector<std::vector<cv::Point> > contoursRed = findContours(segmentedRed); | ||
|
|
||
| //segmentedRed = ip::drawContours(contoursRed, segmentedRed); | ||
|
|
||
| //Get the moments | ||
| std::vector<cv::Moments> muRed = getMu(contoursRed); | ||
|
|
||
| //get the mass centers | ||
| std::vector<cv::Point2i> mcRed = getCenterPoints(muRed, contoursRed); | ||
|
|
||
| //segmentedRed = ip::drawPoints(mcRed, segmentedRed,cv::Vec3b(0,0,255)); | ||
|
|
||
| //DO STUFF WITH RED AND BLUE | ||
| //cv::Mat result = segmentedBlue + segmentedRed; | ||
|
|
||
| std::vector<cv::Point2i> bluePoints = decideOnBlueMarkers(mcBlue, mcRed); | ||
|
|
||
| if (bluePoints.size() < 3 || mcRed.size() < 1) { | ||
| std::cout << "Did not find enough markers" << std::endl; | ||
| std::vector<cv::Point> null; | ||
| cv::Point2i p = {0,0}; | ||
| null.push_back(p); | ||
| return null; | ||
| } | ||
|
|
||
| std::vector<cv::Point2i> allPoints; | ||
|
|
||
| for (int i = 0; i < mcRed.size(); i++) { | ||
| //cv::circle(result, mcRed[i], 10, cv::Vec3b(255, 0, 0), 4); | ||
| allPoints.push_back(mcRed[i]); | ||
| } | ||
| for (int i = 0; i < bluePoints.size(); i++) { | ||
| allPoints.push_back(bluePoints[i]); | ||
| //cv::circle(result, bluePoints[i], 10, cv::Vec3b(0, 0, 255), 4); | ||
| } | ||
|
|
||
| allPoints = toRobotPoints(allPoints, imflip); | ||
|
|
||
| /* Show on QLabel | ||
| QImage img(result.data, result.cols, result.rows, result.step, QImage::Format_RGB888); | ||
| QPixmap p = QPixmap::fromImage(img); | ||
| unsigned int maxW = 400; | ||
| unsigned int maxH = 800; | ||
| _processedPicture->setPixmap(p.scaled(maxW, maxH, Qt::KeepAspectRatio)); | ||
| */ | ||
| return allPoints; | ||
| } | ||
|
|
||
| std::vector<cv::Point2i> ip::marker3Function(const rw::sensor::Image& image, cv::Ptr<cv::xfeatures2d::SURF> _detector, cv::Mat _descriptors_object,std::vector<cv::KeyPoint> _keypoints_object,cv::Mat _img_object) { | ||
| cv::Mat im = toOpenCVImage(image); | ||
| cv::Mat imflip; | ||
| cv::flip(im, imflip, 0); | ||
| cv::cvtColor(imflip, imflip, CV_RGB2GRAY); | ||
|
|
||
| cv::Mat img_scene = imflip; | ||
|
|
||
| //cv::Mat img_object = _img_object.clone(); | ||
|
|
||
| if( !_img_object.data || !img_scene.data ) | ||
| { std::cout<< " --(!) Error reading images " << std::endl; return std::vector<cv::Point2i>(); } | ||
|
|
||
| //-- Step 1: Detect the keypoints using SURF Detector | ||
|
|
||
| std::vector<cv::KeyPoint> keypoints_scene; | ||
|
|
||
| //-- Step 2: Calculate descriptors (feature vectors) | ||
| cv::Mat descriptors_scene; | ||
|
|
||
| _detector->detectAndCompute( img_scene, cv::Mat(), keypoints_scene, descriptors_scene ); | ||
|
|
||
| //-- Step 3: Matching descriptor vectors using FLANN matcher | ||
| cv::FlannBasedMatcher matcher; | ||
| std::vector< cv::DMatch > matches; | ||
| matcher.match( _descriptors_object, descriptors_scene, matches); | ||
|
|
||
| double max_dist = 0; double min_dist = 100; | ||
|
|
||
| //-- Quick calculation of max and min distances between keypoints | ||
| for( int i = 0; i < _descriptors_object.rows; i++ ) | ||
| { | ||
| double dist = matches[i].distance; | ||
| if( dist < min_dist ) | ||
| min_dist = dist; | ||
| if( dist > max_dist ) | ||
| max_dist = dist; | ||
| } | ||
| //-- Draw only "good" matches (i.e. whose distance is less than 3*min_dist ) | ||
| std::vector< cv::DMatch > good_matches; | ||
|
|
||
| for( int i = 0; i < _descriptors_object.rows; i++ ) | ||
| { | ||
| if( matches[i].distance < 3*min_dist) | ||
| good_matches.push_back( matches[i]); | ||
| } | ||
|
|
||
| //-- Localize the object | ||
| std::vector<cv::Point2f> obj; | ||
| std::vector<cv::Point2f> scene; | ||
| for( int i = 0; i < good_matches.size(); i++ ) | ||
| { | ||
| //-- Get the keypoints from the good matches | ||
| obj.push_back( _keypoints_object[ good_matches[i].queryIdx ].pt ); | ||
| scene.push_back( keypoints_scene[ good_matches[i].trainIdx ].pt ); | ||
| } | ||
| cv::Mat H = findHomography( obj, scene, cv::RANSAC ); | ||
| if(H.empty()) | ||
| { | ||
| rw::common::Log::log().info() << "No good matches, no new points calculated" << std::endl; | ||
| std::vector<cv::Point> null; | ||
| cv::Point2i p = {0,0}; | ||
| null.push_back(p); | ||
| return null; | ||
| } | ||
| //-- Get the corners from the image_1 ( the object to be "detected" ) | ||
| std::vector<cv::Point2f> obj_corners(4); | ||
| obj_corners[0] = cvPoint(0,0); | ||
| obj_corners[1] = cvPoint( _img_object.cols, 0 ); | ||
| obj_corners[2] = cvPoint( _img_object.cols, _img_object.rows ); | ||
| obj_corners[3] = cvPoint( 0, _img_object.rows ); | ||
| std::vector<cv::Point2f> scene_corners(4); | ||
|
|
||
| perspectiveTransform( obj_corners, scene_corners, H); | ||
|
|
||
| std::vector<cv::Point2i> points; | ||
| for (int i = 0; i < scene_corners.size(); i++) { | ||
| points.push_back(scene_corners[i]); | ||
| } | ||
| points = toRobotPoints(points, imflip); | ||
| /* | ||
| for(int i = 0; i < points.size(); i++){ | ||
| rw::common::Log::log().info() << "Point: " << i << " " << points[i]; | ||
| } | ||
| rw::common::Log::log().info() << std::endl; | ||
| for (int i = 0; i < points.size(); i++) { | ||
| cv::circle(img_scene,points[i],10,0,4); | ||
| } | ||
| // Show on QLabel | ||
| QImage img(img_scene.data, img_scene.cols, img_scene.rows, img_scene.step, QImage::Format_Grayscale8); | ||
| QPixmap p = QPixmap::fromImage(img); | ||
| unsigned int maxW = 400; | ||
| unsigned int maxH = 800; | ||
| _processedPicture->setPixmap(p.scaled(maxW, maxH, Qt::KeepAspectRatio)); | ||
| */ | ||
| return points; | ||
| } |
| @@ -0,0 +1,33 @@ | ||
| #ifndef IP_H | ||
| #define IP_H | ||
|
|
||
| #include <opencv2/core.hpp> | ||
| #include <opencv2/highgui.hpp> | ||
| #include <opencv2/imgproc.hpp> | ||
|
|
||
| #include "opencv2/xfeatures2d.hpp" | ||
| #include <opencv2/opencv.hpp> | ||
| #include <functional> | ||
| #include <iostream> | ||
| #include <rw/loaders/ImageLoader.hpp> | ||
|
|
||
| class ip { | ||
|
|
||
| public: | ||
| static cv::Mat segmentateHSV(cv::Mat image, int hueMin, int hueMax, int saturationMin, int saturationMax, int valueMin, int valueMax); | ||
| static cv::Mat opening(cv::Mat image, int elementType, int kernelSize); | ||
| static cv::Mat closing(cv::Mat image, int elementType, int kernelSize); | ||
| static std::vector<std::vector<cv::Point> > findContours(cv::Mat image); | ||
| static cv::Mat drawContours(std::vector<std::vector<cv::Point> > contours, cv::Mat image); | ||
| static std::vector<cv::Moments> getMu(std::vector<std::vector<cv::Point> > contours ); | ||
| static std::vector<cv::Point2i> getCenterPoints(std::vector<cv::Moments> moments, std::vector<std::vector<cv::Point> > contours); | ||
| static cv::Mat drawPoints(std::vector<cv::Point2i> points, cv::Mat image, cv::Vec3b color); | ||
| static std::vector<cv::Point2i> toRobotPoints(std::vector<cv::Point2i> points, cv::Mat image); | ||
| static std::vector<cv::Point2i> decideOnBlueMarkers(std::vector<cv::Point2i> mcBlue, std::vector<cv::Point2i> mcRed); | ||
| static std::vector<cv::Point2i> marker1Function(const rw::sensor::Image& image); | ||
| static std::vector<cv::Point2i> marker3Function(const rw::sensor::Image& image, cv::Ptr<cv::xfeatures2d::SURF> _detector, cv::Mat _descriptors_object,std::vector<cv::KeyPoint> _keypoints_object,cv::Mat _img_object); | ||
| static cv::Mat toOpenCVImage(const rw::sensor::Image& img); | ||
| }; | ||
|
|
||
|
|
||
| #endif //IP_H |
| @@ -0,0 +1,92 @@ | ||
| #include "vs.h" | ||
| #include <rw/math.hpp> | ||
| #include <rw/kinematics.hpp> | ||
| #include <rw/loaders.hpp> | ||
|
|
||
| std::vector<double> vs::calcUV(double focal, rw::kinematics::Frame* marker, rw::kinematics::Frame* cam, | ||
| rw::kinematics::State state) { | ||
| // Calculate rCam | ||
| rw::math::Transform3D<> rCamT = cam->fTf(marker, state); | ||
| rw::math::Vector3D<> rCam = rCamT.P(); | ||
|
|
||
| // Calculate u and v from rCam | ||
| std::vector<double> UV = {(focal * rCam[0]) / rCam[2], (focal * rCam[1]) / rCam[2]}; | ||
| UV[0] = -round(UV[0]); | ||
| UV[1] = -round(UV[1]); | ||
| return UV; | ||
| } | ||
|
|
||
| rw::math::Jacobian vs::calcImageJacobian(double u, double v, double z, double focal) { | ||
| rw::math::Jacobian imageJ(2, 6); | ||
| imageJ(0,0) = -(focal/z); | ||
| imageJ(0,1) = 0; | ||
| imageJ(0,2) = u/z; | ||
| imageJ(0,3) = (u*v)/focal; | ||
| imageJ(0,4) = -(pow(focal, 2)+pow(u, 2))/focal; | ||
| imageJ(0,5) = v; | ||
|
|
||
| imageJ(1,0) = 0; | ||
| imageJ(1,1) = -(focal/z); | ||
| imageJ(1,2) = v/z; | ||
| imageJ(1,3) = ((focal*focal)+(v*v))/focal; | ||
| imageJ(1,4) = -(u*v)/focal; | ||
| imageJ(1,5) = -u; | ||
| return imageJ; | ||
| } | ||
|
|
||
| Eigen::Matrix<double, 6, 6> vs::calcS(rw::math::Rotation3D<> camRot) { | ||
| Eigen::Matrix<double, 6, 6> S; | ||
| //rw::common::Log::log().info() << "n: " << camRot << std::endl; | ||
|
|
||
| rw::math::Rotation3D<> rBaseCamTrans = camRot.inverse(); | ||
|
|
||
| //rw::common::Log::log().info() << "i: " << rBaseCamTrans << std::endl; | ||
|
|
||
| S << rBaseCamTrans(0,0), rBaseCamTrans(0,1), rBaseCamTrans(0,2), 0, 0, 0, | ||
| rBaseCamTrans(1,0), rBaseCamTrans(1,1), rBaseCamTrans(1,2), 0, 0, 0, | ||
| rBaseCamTrans(2,0), rBaseCamTrans(2,1), rBaseCamTrans(2,2), 0, 0, 0, | ||
| 0, 0, 0, rBaseCamTrans(0,0), rBaseCamTrans(0,1), rBaseCamTrans(0,2), | ||
| 0, 0, 0, rBaseCamTrans(1,0), rBaseCamTrans(1,1), rBaseCamTrans(1,2), | ||
| 0, 0, 0, rBaseCamTrans(2,0), rBaseCamTrans(2,1), rBaseCamTrans(2,2); | ||
| return S; | ||
| } | ||
|
|
||
| rw::math::Q vs::calcDqFromUV(double u, double v, double du, double dv, double z, double focal, | ||
| rw::kinematics::Frame *camFrame, rw::models::Device::Ptr device, | ||
| rw::kinematics::State state) | ||
| { | ||
| // Calculate image jacobian | ||
| rw::math::Jacobian imageJ = calcImageJacobian(u, v, z, focal); | ||
|
|
||
| // Calculate S | ||
| rw::kinematics::Frame* base = device->getBase(); | ||
| rw::kinematics::FKRange fkCamera(base, camFrame, state); | ||
| rw::math::Transform3D<> baseTcam = fkCamera.get(state); | ||
| Eigen::Matrix<double, 6, 6> S = calcS(baseTcam.R()); | ||
|
|
||
| // Calculate J(q) | ||
| rw::math::Jacobian J = device->baseJframe(camFrame, state); | ||
|
|
||
| // Calculate ZImage | ||
| Eigen::Matrix<double, 2, 7> ZImage = imageJ.e() * S * J.e(); | ||
|
|
||
| //rw::common::Log::log().info() << "ZImage: " << std::endl << ZImage << std::endl; | ||
| //rw::common::Log::log().info() << "ZImage transposed: " << std::endl << ZImage.transpose() << std::endl; | ||
| //rw::common::Log::log().info() << "ZImage*zImageT: " << std::endl << ZImage*ZImage.transpose() << std::endl; | ||
|
|
||
| // Solve for y | ||
| Eigen::Matrix<double, 2, 1> dUdV(du, dv); | ||
| Eigen::Matrix<double, 2, 1> y = rw::math::LinearAlgebra::pseudoInverse(ZImage*ZImage.transpose()) * dUdV; | ||
|
|
||
| //rw::common::Log::log().info() << "dUdV: " << dUdV << std::endl; | ||
| //rw::common::Log::log().info() << "ZImageWeird: " << rw::math::LinearAlgebra::pseudoInverse(ZImage*ZImage.transpose()) << std::endl; | ||
| //rw::common::Log::log().info() << "y: " << y << std::endl; | ||
|
|
||
| // Choose dq | ||
| //rw::common::Log::log().info() << "ZImage: " << std::endl << ZImage << std::endl; | ||
| Eigen::Matrix<double, 7, 1> eQ = ZImage.transpose() * y; | ||
| //rw::common::Log::log().info() << "dQ: " << std::endl << eQ << std::endl; | ||
| rw::math::Q dq(eQ); | ||
|
|
||
| return dq; | ||
| } |
| @@ -0,0 +1,23 @@ | ||
| #ifndef VS_H | ||
| #define VS_H | ||
|
|
||
| #include <rw/models.hpp> | ||
|
|
||
| class vs { | ||
|
|
||
| public: | ||
| static std::vector<double> calcUV(double focal, rw::kinematics::Frame* marker, rw::kinematics::Frame* cam, | ||
| rw::kinematics::State state); | ||
|
|
||
| static rw::math::Jacobian calcImageJacobian(double u, double v, double z, double focal); | ||
|
|
||
| static Eigen::Matrix<double, 6, 6> calcS(rw::math::Rotation3D<> camRot); | ||
|
|
||
| static rw::math::Q calcDqFromUV(double u, double v, double du, double dv, double z, double focal, | ||
| rw::kinematics::Frame* camFrame, rw::models::Device::Ptr device, | ||
| rw::kinematics::State state); | ||
|
|
||
| }; | ||
|
|
||
|
|
||
| #endif //VS_H |
| @@ -0,0 +1,155 @@ | ||
| #include "vsMult.h" | ||
| #include "vs.h" | ||
| #include <rw/math.hpp> | ||
| #include <rw/kinematics.hpp> | ||
| #include <rw/loaders.hpp> | ||
|
|
||
| std::vector<std::vector<double>> vsMult::calcUVMult(double focal, rw::kinematics::Frame *p1, rw::kinematics::Frame *p2, | ||
| rw::kinematics::Frame *p3, rw::kinematics::Frame *cam, | ||
| rw::kinematics::State state) { | ||
| // Init | ||
| std::vector<double> U; | ||
| std::vector<double> V; | ||
| std::vector<std::vector<double>> UV; | ||
|
|
||
| // Calc point 1 | ||
| rw::math::Transform3D<> rCamTp1 = cam->fTf(p1, state); | ||
| rw::math::Vector3D<> rCamP1 = rCamTp1.P(); | ||
| //std::vector<double> UVp1 = {(focal * rCamP1[0]) / rCamP1[2], (focal * rCamP1[1]) / rCamP1[2]}; | ||
| //UVp1[0] = -round(UVp1[0]); | ||
| //UVp1[1] = -round(UVp1[1]); | ||
|
|
||
| U.push_back(-round((focal * rCamP1[0]) / rCamP1[2])); | ||
| V.push_back(-round((focal * rCamP1[1]) / rCamP1[2])); | ||
|
|
||
| // Calc point 2 | ||
| rw::math::Transform3D<> rCamTp2 = cam->fTf(p2, state); | ||
| rw::math::Vector3D<> rCamP2 = rCamTp2.P(); | ||
| //std::vector<double> UVp2 = {(focal * rCamP2[0]) / rCamP2[2], (focal * rCamP2[1]) / rCamP2[2]}; | ||
| //UVp2[0] = -round(UVp2[0]); | ||
| //UVp2[1] = -round(UVp2[1]); | ||
|
|
||
| U.push_back(-round((focal * rCamP2[0]) / rCamP2[2])); | ||
| V.push_back(-round((focal * rCamP2[1]) / rCamP2[2])); | ||
|
|
||
| // Calc point 3 | ||
| rw::math::Transform3D<> rCamTp3 = cam->fTf(p3, state); | ||
| rw::math::Vector3D<> rCamP3 = rCamTp3.P(); | ||
| //std::vector<double> UVp3 = {(focal * rCamP3[0]) / rCamP3[2], (focal * rCamP3[1]) / rCamP3[2]}; | ||
| //UVp3[0] = -round(UVp3[0]); | ||
| //UVp3[1] = -round(UVp3[1]); | ||
|
|
||
| U.push_back(-round((focal * rCamP3[0]) / rCamP3[2])); | ||
| V.push_back(-round((focal * rCamP3[1]) / rCamP3[2])); | ||
|
|
||
| UV.push_back(U); | ||
| UV.push_back(V); | ||
|
|
||
| //std::cout << "size: " << U.size() << std::endl; | ||
| //std::cout << "size: " << V.size() << std::endl; | ||
|
|
||
| //std::cout << "size: " << UV[0].size() << std::endl; | ||
| //std::cout << "size: " << UV[1].size() << std::endl; | ||
|
|
||
| return UV; | ||
| } | ||
|
|
||
| rw::math::Jacobian vsMult::calcImageJacobian(std::vector<double> u, std::vector<double> v, double z, double focal) { | ||
|
|
||
| if (u.size() != v.size()) | ||
| return rw::math::Jacobian(0, 0); | ||
|
|
||
| rw::math::Jacobian imageJ(2*u.size(), 6); | ||
|
|
||
| for (int i = 0; i < u.size(); i++) { | ||
| imageJ(i*2,0) = -(focal/z); | ||
| imageJ(i*2,1) = 0; | ||
| imageJ(i*2,2) = u[i]/z; | ||
| imageJ(i*2,3) = (u[i]*v[i])/focal; | ||
| imageJ(i*2,4) = -(pow(focal, 2)+pow(u[i], 2))/focal; | ||
| imageJ(i*2,5) = v[i]; | ||
|
|
||
| imageJ((i*2)+1,0) = 0; | ||
| imageJ((i*2)+1,1) = -(focal/z); | ||
| imageJ((i*2)+1,2) = v[i]/z; | ||
| imageJ((i*2)+1,3) = ((focal*focal)+(v[i]*v[i]))/focal; | ||
| imageJ((i*2)+1,4) = -(u[i]*v[i])/focal; | ||
| imageJ((i*2)+1,5) = -u[i]; | ||
| } | ||
|
|
||
| return imageJ; | ||
| } | ||
|
|
||
| rw::math::Q | ||
| vsMult::calcDqFromUV(std::vector<double> u, std::vector<double> v, std::vector<double> du, std::vector<double> dv, | ||
| double z, double focal, rw::kinematics::Frame *camFrame, | ||
| rw::models::Device::Ptr device, rw::kinematics::State state) { | ||
|
|
||
| // Calculate image jacobian | ||
| rw::math::Jacobian imageJ = calcImageJacobian(u, v, z, focal); | ||
|
|
||
| // Calculate S | ||
| rw::kinematics::Frame* base = device->getBase(); | ||
| rw::kinematics::FKRange fkCamera(base, camFrame, state); | ||
| rw::math::Transform3D<> baseTcam = fkCamera.get(state); | ||
| Eigen::Matrix<double, 6, 6> S = vs::calcS(baseTcam.R()); | ||
|
|
||
| // Calculate J(q) | ||
| rw::math::Jacobian J = device->baseJframe(camFrame, state); | ||
|
|
||
| // Calculate ZImage | ||
| Eigen::MatrixXd ZImage; | ||
| ZImage = imageJ.e() * S * J.e(); | ||
|
|
||
| //rw::common::Log::log().info() << "ZImage: " << std::endl << ZImage << std::endl; | ||
| //rw::common::Log::log().info() << "ZImage transposed: " << std::endl << ZImage.transpose() << std::endl; | ||
| //rw::common::Log::log().info() << "ZImage*zImageT: " << std::endl << ZImage*ZImage.transpose() << std::endl; | ||
|
|
||
| // Solve for y | ||
| Eigen::MatrixXd dUdV(du.size()*2, 1); | ||
| for (int i = 0; i < du.size(); i++) { | ||
| dUdV(i*2, 0) = du[i]; | ||
| dUdV((i*2)+1, 0) = dv[i]; | ||
| } | ||
| //rw::common::Log::log().info() << "dUdV: " << std::endl << dUdV << std::endl; | ||
| Eigen::MatrixXd y; | ||
| y = rw::math::LinearAlgebra::pseudoInverse(ZImage*ZImage.transpose()) * dUdV; | ||
|
|
||
| //rw::common::Log::log().info() << "dUdV: " << dUdV << std::endl; | ||
| //rw::common::Log::log().info() << "ZImageWeird: " << rw::math::LinearAlgebra::pseudoInverse(ZImage*ZImage.transpose()) << std::endl; | ||
| //rw::common::Log::log().info() << "y: " << y << std::endl; | ||
|
|
||
| //SVD METHODS, SEEMS NOT WORK WITH CURRENT CODE | ||
| //Eigen::JacobiSVD<Eigen::MatrixXd> svd1(ZImage, Eigen::ComputeThinU | Eigen::ComputeThinV); | ||
| //Eigen::MatrixXd dq1_e = svd1.solve(dUdV); | ||
| //rw::common::Log::log().info() << "SVD dQ: " << std::endl << dq1_e << std::endl; | ||
|
|
||
| // Choose dq | ||
| //rw::common::Log::log().info() << "ZImage: " << std::endl << ZImage << std::endl; | ||
| Eigen::Matrix<double, 7, 1> eQ = ZImage.transpose() * y; | ||
| //rw::common::Log::log().info() << "dQ: " << std::endl << eQ << std::endl; | ||
| rw::math::Q dq(eQ); | ||
|
|
||
| return dq; | ||
| } | ||
|
|
||
| std::vector<std::vector<double>> | ||
| vsMult::calcDuDv(std::vector<std::vector<double>> from, std::vector<std::vector<double>> to) { | ||
|
|
||
| std::vector<double> du; | ||
| std::vector<double> dv; | ||
| std::vector<std::vector<double>> out; | ||
|
|
||
| //std::cout << "size: " << from[0].size() << std::endl; | ||
|
|
||
| // Fix | ||
| for (int i = 0; i < from[0].size(); i++) { | ||
| du.push_back(to[0][i] - from[0][i]); | ||
| dv.push_back(to[1][i] - from[1][i]); | ||
| } | ||
|
|
||
| out.push_back(du); | ||
| out.push_back(dv); | ||
|
|
||
| return out; | ||
| } |
| @@ -0,0 +1,25 @@ | ||
| #ifndef VSMULT_H | ||
| #define VSMULT_H | ||
|
|
||
| #include <rw/models.hpp> | ||
|
|
||
| class vsMult { | ||
|
|
||
| public: | ||
| static std::vector<std::vector<double>> calcUVMult(double focal, rw::kinematics::Frame *p1, | ||
| rw::kinematics::Frame *p2, rw::kinematics::Frame *p3, | ||
| rw::kinematics::Frame *cam, rw::kinematics::State state); | ||
|
|
||
| static rw::math::Jacobian calcImageJacobian(std::vector<double> u, std::vector<double> v, double z, double focal); | ||
|
|
||
| static rw::math::Q calcDqFromUV(std::vector<double> u, std::vector<double> v, std::vector<double> du, | ||
| std::vector<double> dv, double z, double focal, | ||
| rw::kinematics::Frame* camFrame, rw::models::Device::Ptr device, | ||
| rw::kinematics::State state); | ||
|
|
||
| static std::vector<std::vector<double>> calcDuDv(std::vector<std::vector<double>> from, | ||
| std::vector<std::vector<double>> to); | ||
| }; | ||
|
|
||
|
|
||
| #endif //VS_H |
