AUV-Coop-Assembly
Master Thesis for Robotics Engineering. Cooperative peg-in-hole assembly with two underwater vehicles guided by vision
vision_old.cpp
1 #include <ros/ros.h>
2 #include <iostream>
3 #include <opencv2/imgcodecs.hpp>
4 #include <opencv2/calib3d.hpp>
5 #include <opencv2/highgui.hpp>
6 #include <opencv2/imgproc/imgproc.hpp>
7 
8 int main(int argc, char **argv){
9 
10  std::string left_im = "/home/tori/UWsim/Peg/cameraL_front.png";
11  std::string right_im = "/home/tori/UWsim/Peg/cameraR_front.png";
12 
13  cv::Mat left = cv::imread(left_im , cv::IMREAD_COLOR);
14  if ( left.empty() )
15  {
16  std::cout<<"Cannot read image file: "<<left_im;
17  return -1;
18  }
19 
20  cv::Mat right = cv::imread(right_im, cv::IMREAD_COLOR);
21  if ( right.empty() )
22  {
23  std::cout<<"Cannot read image file: "<<right_im;
24  return -1;
25  }
26 
27 
28  //convert from colors to single channel
29  cv::Mat right_HSV;
30  cv::Mat right_GRAY;
31  cv::Mat right_single;
32  //cv::cvtColor(right, right_HSV, cv::COLOR_BGR2HSV);
33  cv::cvtColor(right, right_single, cv::COLOR_BGR2GRAY);
34  cv::cvtColor(right, right_GRAY, cv::COLOR_BGR2GRAY);
35 
36  cv::Mat left_HSV;
37  cv::Mat left_GRAY;
38  cv::Mat left_single;
39  //cv::cvtColor(left, left_HSV, cv::COLOR_BGR2HSV);
40  cv::cvtColor(left, left_single, cv::COLOR_BGR2GRAY);
41  cv::cvtColor(left, left_GRAY, cv::COLOR_BGR2GRAY);
42 
43 
44  //Apply a Median blur to reduce noise and avoid false circle detection:
45  cv::medianBlur(right_single, right_single, 3);
46  cv::medianBlur(left_single, left_single, 3);
47 
48  cv::imshow("blurredRight", right_single);
49  cv::waitKey(0);
50  cv::imshow("blurredLeft", left_single);
51  cv::waitKey(0);
52 
53  std::vector<cv::Vec3f> circles;
54  cv::HoughCircles(right_single, circles, cv::HOUGH_GRADIENT, 1,
55  right_single.rows/16, // change this value to detect circles with different distances to each other
56  100, 15, 1, 30 // change the last two parameters
57  // (min_radius & max_radius) to detect larger circles
58  );
59 
60 
61  for( size_t i = 0; i < circles.size(); i++ )
62  {
63  std::cout << "detected" << "\n";
64  cv::Vec3i c = circles[i];
65  cv::Point center = cv::Point(c[0], c[1]);
66  // circle center
67  cv::circle( right_single, center, 1, cv::Scalar(0,100,100), 1, cv::LINE_AA);
68  // circle outline
69  int radius = c[2];
70  cv::circle( right_single, center, radius, cv::Scalar(255,0,255), 1, cv::LINE_AA);
71  }
72  cv::imshow("detected circles right", right_single);
73  cv::waitKey();
74 
75  std::vector<cv::Vec3f> circles2;
76  cv::HoughCircles(left_single, circles2, cv::HOUGH_GRADIENT, 1,
77  left_single.rows/16, // change this value to detect circles with different distances to each other
78  100, 15, 1, 30 // change the last two parameters
79  // (min_radius & max_radius) to detect larger circles
80  );
81  for( size_t i = 0; i < circles2.size(); i++ )
82  {
83  std::cout << "detected" << "\n";
84  cv::Vec3i c = circles2[i];
85  cv::Point center = cv::Point(c[0], c[1]);
86  // circle center
87  cv::circle( left_single, center, 1, cv::Scalar(0,100,100), 1, cv::LINE_AA);
88  // circle outline
89  int radius = c[2];
90  cv::circle( left_single, center, radius, cv::Scalar(255,0,255), 1, cv::LINE_AA);
91  }
92  cv::imshow("detected circles left", left_single);
93  cv::waitKey();
94 
95 
96  int numDisparities=48;
97  int blockSize=std::atoi(argv[1]);
98  cv::Ptr<cv::StereoMatcher> stereo = cv::StereoBM::create(numDisparities, blockSize);
99  cv::Mat disparity;
100  stereo->compute(left_GRAY, right_GRAY, disparity);
101 
103  cv::Mat intrParam = (cv::Mat_<double>(3,3) <<
104  257.34082279179285, 0.0, 160.0,
105  0.0, 257.34083046114705, 120.0,
106  0.0, 0.0, 1.0);
107 
108  cv::Mat lRr = cv::Mat::eye(3,3, CV_64F);
109  cv::Mat lDistr = (cv::Mat_<double>(3,1) << 0.19999999999999984, 0.0, 0.0);
110  cv::Mat distorsionParam = cv::Mat::zeros(5,1, CV_64F) ;
111  cv::Size imageSize(right_single.rows, right_single.cols);
112 
113 
114  // output matrices of stereorect
115  cv::Mat lRectR = cv::Mat::zeros(3,3, CV_64F);
116  cv::Mat rRectR = cv::Mat::zeros(3,3, CV_64F);
117  cv::Mat lProject = cv::Mat::zeros(3,4, CV_64F); //the 3x4 P matrix
118  cv::Mat rProject = cv::Mat::zeros(3,4, CV_64F);
119  cv::Mat disp2depthMap = cv::Mat::zeros(4,4, CV_64F);
120  cv::stereoRectify(intrParam, distorsionParam, intrParam, distorsionParam,
121  imageSize, lRr, lDistr,
122  lRectR, rRectR, lProject, rProject ,
123  disp2depthMap);
124 
125  //project center of circle in common
126  std::cout << "lRectR:\n" << lRectR << "\n\n";
127  std::cout << "rRectR:\n" << rRectR << "\n\n";
128  std::cout << "lProject:\n" << lProject << "\n\n";
129  std::cout << "rProject:\n" << rProject << "\n\n";
130  std::cout << "disp2depthMap (Q):\n" << disp2depthMap << "\n\n";
131 
132 
134  cv::Mat disp8U;
135  disparity.convertTo(disp8U, CV_8U);
136  cv::imshow("disparity", disp8U);
137  cv::waitKey(0);
138 
139 
140  cv::Mat disp8UFilt;
141  cv::medianBlur(disp8U, disp8UFilt, 3);
142  cv::imshow("disp blurred", disp8UFilt);
143  cv::waitKey(0);
144  cv::Mat disp8UCirc = disp8UFilt;
145  for( size_t i = 0; i < circles2.size(); i++ )
146  {
147  cv::Vec3i c = circles2[i];
148  cv::Point center = cv::Point(c[0], c[1]);
149  // circle center
150  cv::circle( disp8UCirc, center, 1, cv::Scalar(0,100,100), 1, cv::LINE_AA);
151  // circle outline
152  int radius = c[2];
153  cv::circle( disp8UCirc, center, radius, cv::Scalar(255,0,255), 3, cv::LINE_AA);
154  }
155  cv::imshow("disparityCirc", disp8UCirc);
156  cv::waitKey(0);
157 
158 
159 
160 // /// reporject imaget to 3D method
161 // cv::Mat _3dImage(disparity.size(), CV_64FC3); //C3 stand for 3channels
162 // cv::reprojectImageTo3D(disparity, _3dImage, disp2depthMap);
163 
164 
165 // cv::imshow("final", _3dImage);
166 // cv::waitKey(0);
167 
168 // cv::Vec3i c = circles2[0];
169 // std::cout << c[0] << "\n";
170 // std::cout << "3D POIMT " << _3dImage.at<cv::Vec3d>(c[1], c[0]) << std::endl;
171 
172 // std::cout << "3D corner bottom left" << _3dImage.at<cv::Vec3d>(196, 106) << std::endl;
173 
174 
175 
177  cv::Mat pnts3D;
178  //cv::Point2f centerL = cv::Point(circles2[0][0], circles2[0][1]);
179  //cv::Point2f centerR = cv::Point(circles[0][0], circles[0][1]);
180  cv::Mat centerL(1,1,CV_32FC2);
181  cv::Mat centerR(1,1,CV_32FC2);
182  centerL.at<cv::Vec2f>(0,0)[0] = circles2[0][0];
183  centerL.at<cv::Vec2f>(0,0)[1] = circles2[0][1];
184  centerR.at<cv::Vec2f>(0,0)[0] = circles[0][0];
185  centerR.at<cv::Vec2f>(0,0)[1] = circles[0][1];
186 
187 
188  //std::cout << centerL << "\n";
189 
190  cv::triangulatePoints( lProject, rProject, centerL, centerR, pnts3D );
191 
192 
193  cv::Mat t = pnts3D.t();
194  cv::Mat pnts3DT = cv::Mat( 1, 1, CV_32FC4, t.data );
195 
196  cv::Mat resultPoints;
197  cv::convertPointsFromHomogeneous( pnts3DT, resultPoints );
198  std::cout << "3D POIMT triangulat " << pnts3DT << std::endl;
199 
200 
202  std::vector<cv::Vec3f> surfacePoints, realSurfacePoints;
203 
204  unsigned int N = 1;
205  for(int i=0;i<N;i++) {
206  double d, disparity;
207  // since you have stereo vision system in which cameras lays next to
208  // each other on OX axis, disparity is measured along OX axis
209  //d = T.at<double>(0,0);
210  disparity = circles2[0][0] - circles[0][0];
211  std::cout << "asdas\n" << disparity << "\n\n";
212 
213  surfacePoints.push_back(cv::Vec3f(circles2[0][0], circles2[0][1], disparity));
214  }
215 
216  cv::perspectiveTransform(surfacePoints, realSurfacePoints, disp2depthMap);
217  std::cout << "3D POIMT tizio " << realSurfacePoints.at(0) << std::endl;
218 
219 
220 
222  // corners per le imaggine front_LEFT
223  // bottom left x,y = 106,196
224  //bottom right x,y = 252, 202
225 
226 
227  return 0;
228 }
229 
Generated by   doxygen 1.8.11