├── .gitignore
├── CMakeLists.txt
├── README.md
├── main.cpp
└── data.yml


/.gitignore:
--------------------------------------------------------------------------------
1 | .vscode
2 | build
3 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.5)
2 | project(TestHandeye)
3 | 
4 | find_package(OpenCV REQUIRED)
5 | 
6 | add_executable(${PROJECT_NAME} main.cpp)
7 | target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS})


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # opencv-handeye
 2 | 
 3 | Build:
 4 | ```
 5 | cd ~/git/opencv-handeye
 6 | docker run -v $(pwd):/root/opencv-handeye -it jjanzic/docker-python3-opencv bash
 7 | # Now you are inside the docker container
 8 | cd /root/opencv-handeye
 9 | mkdir build && cd build
10 | cmake ..
11 | make
12 | ```
13 | 
14 | Run:
15 | ```
16 | docker run -v $(pwd):/root/opencv-handeye -it jjanzic/docker-python3-opencv bash
17 | cd /root/opencv-handeye/build
18 | ./TestHandeye
19 | ```
20 | 
21 | Expected output:
22 | ```
23 | ---
24 | OpenCV version : 4.1.0
25 | Major version : 4
26 | Minor version : 1
27 | Subminor version : 0
28 | ---
29 | Num of gripper2base transforms: 10
30 | Num of target2cam transforms: 10
31 | ---
32 | Estimated cam2gripper: 
33 | [0.8357872530342421, 0.4275441928943817, 0.344478781331692, 30.08659781703733;
34 |  -0.410875161208457, 0.90320366951094, -0.1241158059390394, 88.40409398451877;
35 |  -0.3641994914430832, -0.03780336630863101, 0.9305534030502145, 297.8756595212807;
36 |  0, 0, 0, 0]
37 | ```


--------------------------------------------------------------------------------
/main.cpp:
--------------------------------------------------------------------------------
 1 | #include <opencv2/opencv.hpp>
 2 | #include <opencv2/calib3d.hpp>
 3 | 
 4 | #include <iostream>
 5 | 
 6 | int main(int, char**)
 7 | {
 8 |     std::cout << "---\n";
 9 |     std::cout << "OpenCV version : " << CV_VERSION << '\n';
10 |     std::cout << "Major version : " << CV_MAJOR_VERSION << '\n';
11 |     std::cout << "Minor version : " << CV_MINOR_VERSION << '\n';
12 |     std::cout << "Subminor version : " << CV_SUBMINOR_VERSION << '\n';
13 |     // Read the list of transforms
14 |     std::vector<cv::Mat> R_gripper2base, t_gripper2base;
15 |     std::vector<cv::Mat> R_target2cam, t_target2cam;
16 |     cv::FileStorage fs("../data.yml", cv::FileStorage::READ);
17 |     cv::FileNode fn = fs.root();
18 |     cv::Rect rotation_rect(0,0,3,3);
19 |     cv::Rect translation_rect(3,0,1,3);
20 |     for (cv::FileNodeIterator it = fn.begin(); it != fn.end(); ++it)
21 |     {
22 |         std::string node_name = (*it).name();
23 |         cv::Mat T;
24 |         fs[node_name] >> T;
25 |         cv::Mat rotation = T(rotation_rect);
26 |         cv::Mat translation = T(translation_rect);
27 |         if (node_name.find("gripper2base") != std::string::npos) {
28 |             R_gripper2base.push_back(rotation);
29 |             t_gripper2base.push_back(translation);
30 |         }
31 |         else if (node_name.find("target2cam") != std::string::npos) {
32 |             R_target2cam.push_back(rotation);
33 |             t_target2cam.push_back(translation);
34 |         }
35 |     }
36 |     std::cout << "---\n";
37 |     std::cout << "Num of gripper2base transforms: " << R_gripper2base.size() << '\n';
38 |     std::cout << "Num of target2cam transforms: " << R_gripper2base.size() << '\n';
39 |     std::cout << "---\n";
40 |     // Calibrate
41 |     cv::Mat R_cam2gripper, t_cam2gripper;
42 |     cv::calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper, t_cam2gripper);
43 |     //  Output the results
44 |     cv::Mat T_cam2gripper = cv::Mat::zeros(4, 4, CV_64F);
45 |     R_cam2gripper.copyTo(T_cam2gripper(rotation_rect));
46 |     t_cam2gripper.copyTo(T_cam2gripper(translation_rect));
47 |     std::cout << "Estimated cam2gripper: \n" << T_cam2gripper << '\n';
48 | }


--------------------------------------------------------------------------------
/data.yml:
--------------------------------------------------------------------------------
  1 | %YAML:1.0
  2 | ---
  3 | target2cam01: !!opencv-matrix
  4 |   rows: 4
  5 |   cols: 4
  6 |   dt: d
  7 |   data: [0.03217746144087852, -0.9888245700192507, -0.1455698481879555, 135.9560532527401,
  8 |         0.9144789884832837, 0.08790366423949575, -0.3949697778765613, -41.23644292334733,
  9 |         0.4033519438378982, -0.1204114427266513, 0.9070878093452005, 380.9433193025574,
 10 |         0, 0, 0, 1]
 11 | target2cam02: !!opencv-matrix
 12 |   rows: 4
 13 |   cols: 4
 14 |   dt: d
 15 |   data: [0.06262272047439971, -0.9974794311039886, -0.03336434331510531, 113.4949568630841,
 16 |         0.9832880080544576, 0.06738879033592499, -0.1691255277991984, -64.64482171271662,
 17 |         0.1709476179906637, -0.02221565802590043, 0.9850296322658494, 347.6400735663287,
 18 |         0, 0, 0, 1]
 19 | 
 20 | target2cam03: !!opencv-matrix
 21 |   rows: 4
 22 |   cols: 4
 23 |   dt: d
 24 |   data: [-0.01170205985521532, -0.8988526682940601, -0.4380946729826708, 180.7241090775647,
 25 |         0.9946406950906383, 0.03454706683583417, -0.0974494117101376, -53.95226323758228,
 26 |         0.1027275496872989, -0.4368871488996771, 0.8936311709321436, 514.3437518844075,
 27 |         0, 0, 0, 1]
 28 | 
 29 | target2cam04: !!opencv-matrix
 30 |   rows: 4
 31 |   cols: 4
 32 |   dt: d
 33 |   data: [-0.06898233789767504, -0.9047595675739113, -0.4202993718073343, 166.7242487150877,
 34 |         0.9860077981677604, 0.002254781093836908, -0.16668394618134, 24.26455244711471,
 35 |         0.151756578145845, -0.4259167064646416, 0.8919444490234669, 615.1215068183059,
 36 |         0, 0, 0, 1]
 37 | 
 38 | target2cam05: !!opencv-matrix
 39 |   rows: 4
 40 |   cols: 4
 41 |   dt: d
 42 |   data: [0.5434307501563939, 0.7979593133074483, 0.2606606109299305, -267.6140689241807,
 43 |         -0.7547526615631706, 0.6003715259706235, -0.2643907159243966, 14.11279934722869,
 44 |         -0.3674662428683335, -0.0530562447748662, 0.9285222642686712, 520.6050361002349,
 45 |         0, 0, 0, 1]
 46 | 
 47 | target2cam06: !!opencv-matrix
 48 |   rows: 4
 49 |   cols: 4
 50 |   dt: d
 51 |   data: [0.9871693099045882, 0.001300488875197107, -0.1596717329748271, 102.1425389768893,
 52 |         0.01327827186690634, 0.9958348193196344, 0.09020365916557821, -119.2282731863266,
 53 |         0.1591239802126868, -0.09116644864925236, 0.983040303121879, 413.4585766667393,
 54 |         0, 0, 0, 1]
 55 | 
 56 | target2cam07: !!opencv-matrix
 57 |   rows: 4
 58 |   cols: 4
 59 |   dt: d
 60 |   data: [0.6882952950170128, 0.004835866353914343, -0.7254145030629389, -269.9554746983414,
 61 |         0.06985057147551049, 0.9948895568525418, 0.07290862315458643, -113.4986445777273,
 62 |         0.7220598898443176, -0.10085327987904, 0.6844400130147653, 481.9916762922463,
 63 |         0, 0, 0, 1]
 64 | 
 65 | target2cam08: !!opencv-matrix
 66 |   rows: 4
 67 |   cols: 4
 68 |   dt: d
 69 |   data: [0.6493859700308469, -0.1559757742586124, -0.7442912197329231, -237.3093394017367,
 70 |         0.07085626403518119, 0.98689207221228, -0.1449945780073471, -132.8890823788564,
 71 |         0.7571507457396266, 0.04141974950405133, 0.651925726273284, 485.900134488424,
 72 |         0, 0, 0, 1]
 73 | 
 74 | target2cam09: !!opencv-matrix
 75 |   rows: 4
 76 |   cols: 4
 77 |   dt: d
 78 |   data: [0.9297765056561516, -0.2320365916051094, -0.2857878053488634, 61.45846604336563,
 79 |         0.1703487805178876, 0.9594097441656958, -0.2247537224963864, -103.3370599036759,
 80 |         0.3263386929340508, 0.1602871266078402, 0.9315637898383006, 373.1894219136454,
 81 |         0, 0, 0, 1]
 82 | 
 83 | target2cam10: !!opencv-matrix
 84 |   rows: 4
 85 |   cols: 4
 86 |   dt: d
 87 |   data: [0.05473079479085888, -0.9693463386048708, 0.2395249797722399, 179.0897632845463,
 88 |         0.9177411165381688, -0.04567456870424201, -0.39454413794778, -73.16917250140072,
 89 |         0.3933901156826849, 0.2414156366249141, 0.887109185656507, 344.6305321777664,
 90 |         0, 0, 0, 1]
 91 |   
 92 | gripper2base01: !!opencv-matrix
 93 |   rows: 4
 94 |   cols: 4
 95 |   dt: d
 96 |   data: [-0.03297927740105067, -0.8931522828781312, 0.4485436348749427,  781.20001,
 97 |         -0.9889760982746338, 0.09398301747812331, 0.1144268377489601,  338.59,
 98 |         -0.1443560719490051, -0.4398251940045839, -0.8864057005072823, 903.47998,
 99 |         0, 0, 0, 1]
100 | 
101 | gripper2base02: !!opencv-matrix
102 |   rows: 4
103 |   cols: 4
104 |   dt: d
105 |   data: [-0.05765560762676536, -0.9708910708322454, 0.2324786752851849,  867.65002,
106 |         -0.9969269266810485, 0.06836159770420459, 0.03825399019082287,  382.51999,
107 |         -0.05303307250142097, -0.2295586902715012, -0.9718488641147474,  884.41998,
108 |         0, 0, 0, 1]
109 | 
110 | gripper2base03: !!opencv-matrix
111 |   rows: 4
112 |   cols: 4
113 |   dt: d
114 |   data: [0.02706358395355735, -0.9866644894830339, 0.1605016213248231,  856.90997,
115 |         -0.9064932265676475, 0.04345372413264977, 0.4199783303475524,  172.99001,
116 |         -0.4213520884513855, -0.1568597499959719, -0.8932286551839574,  964.60999,
117 |         0, 0, 0, 1]
118 | 
119 | gripper2base04: !!opencv-matrix
120 |   rows: 4
121 |   cols: 4
122 |   dt: d
123 |   data: [0.08156515488912897, -0.9743977416549333, 0.2095140999625946,  748.81,
124 |         -0.9049858960028772, 0.01566124909525608, 0.4251532369944285,  146.75,
125 |         -0.4175496101379395, -0.2242850016097604, -0.8805388266164726,  1043.29,
126 |         0, 0, 0, 1]
127 | 
128 | gripper2base05: !!opencv-matrix
129 |   rows: 4
130 |   cols: 4
131 |   dt: d
132 |   data: [0.5343486310554724, -0.7303756133659112, 0.4254679169390385,  627.65997,
133 |         -0.7994058389699532, -0.6002124176111558, -0.02636888730895692,  554.08002,
134 |         0.2746303379535675, -0.3260313629223432, -0.9045892265963928,  920.84998,
135 |         0, 0, 0, 1]
136 | 
137 | gripper2base06: !!opencv-matrix
138 |   rows: 4
139 |   cols: 4
140 |   dt: d
141 |   data: [-0.9817933440208435, -0.01430648109786638, 0.1894126013753931,  715.06,
142 |         2.48902451209023e-06, 0.9971587424641397, 0.07532907686632009, 195.96001,
143 |         -0.1899521201848984, 0.07395805773071995, -0.9790037806680125, 889.38,
144 |         0, 0, 0, 1]
145 | 
146 | gripper2base07: !!opencv-matrix
147 |   rows: 4
148 |   cols: 4
149 |   dt: d
150 |   data: [-0.6974151039926824, -0.0547574213646869, 0.7145725110381964,  790.90002,
151 |         -0.0009756514141850622, 0.9971484899981754, 0.07545888642769476,  196.28999,
152 |         -0.7166667580604553, 0.05192899086230662, -0.6954798074246327,  1117.38,
153 |         0, 0, 0, 1]
154 | 
155 | gripper2base08: !!opencv-matrix
156 |   rows: 4
157 |   cols: 4
158 |   dt: d
159 |   data: [-0.6578525149713528, -0.04908102468794853, 0.7515457981085574,  743.5,
160 |         -0.1609771442629793, 0.9839773177145608, -0.07664815423806592,  283.92999,
161 |         -0.7357420921325684, -0.1714048822914123, -0.6552128950391136,  1131.92,
162 |         0, 0, 0, 1]
163 | 
164 | gripper2base09: !!opencv-matrix
165 |   rows: 4
166 |   cols: 4
167 |   dt: d
168 |   data: [-0.923745679462229, -0.1572344666085333, 0.3492438393080218,  748.90002,
169 |         -0.2274090830285402, 0.9588825937658753, -0.1697918522630138,  288.19,
170 |         -0.3081867098808289, -0.2362657070323237, -0.9215201827728947,  910.58002,
171 |         0, 0, 0, 1]
172 | 
173 | gripper2base10: !!opencv-matrix
174 |   rows: 4
175 |   cols: 4
176 |   dt: d
177 |   data: [-0.07070671020716013, -0.8891483182577163, 0.4521236941817913,  813.17999,
178 |         -0.9651667442976475, -0.05347860948021664, -0.2561117261388871,  400.44,
179 |         0.2519002556800842, -0.4544835880147069, -0.8543951219701817,  917.15997,
180 |         0, 0, 0, 1]


--------------------------------------------------------------------------------