目标相对位置(返回角度和距离).md

1. //代码为矿大大疆创新实验室比赛时所写，copy一份
2. #include <iostream>
3. #include "opencv2/opencv.hpp"
4. #include "opencv2/imgproc/imgproc.hpp"
5. #include "opencv2/core/core.hpp"
6. #include<stdio.h>
7. #include<math.h>
9. #define IMAGE_WIDTH 640 //图像宽度
10. #define IMAGE_HEIGHT 480 //图像高度
11. #define pi 3.14159265 //图像高度
13. using namespace cv;
14. using namespace std;
16. int main( int argc, char** argv )
17. {
18. // VideoCapture cap("2.avi");
19. VideoCapture cap(0);
20. if ( !cap.isOpened() )
21. {
22. cout << "Cannot open" << endl;
23. return -1;
24. }
25. /***************HSV设置滑动块*************************/
26. int iLowH = 90;
27. int iHighH = 110;
29. int iLowS = 90;
30. int iHighS = 255;
32. int iLowV = 90;
33. int iHighV = 255;
35. namedWindow("Control", CV_WINDOW_AUTOSIZE); //建立调整HSV的窗口"Control"
36. cvCreateTrackbar("LowH", "Control", &iLowH, 179); //Hue (0 - 179)
37. cvCreateTrackbar("HighH", "Control", &iHighH, 179);
38. cvCreateTrackbar("LowS", "Control", &iLowS, 255); //Saturation (0 - 255)
39. cvCreateTrackbar("HighS", "Control", &iHighS, 255);
40. cvCreateTrackbar("LowV", "Control", &iLowV, 255); //Value (0 - 255)
41. cvCreateTrackbar("HighV", "Control", &iHighV, 255);
42. /*************************************************/
43. //Size s(640, 480);
44. //VideoWriter writer=VideoWriter("imgThresholded.avi",CV_FOURCC('M','J','P','G'), 25, s);//写视频
46. //对视频处理并显示
47. while (true)
48. {
49. Mat imgOriginal;
50. bool bSuccess = cap.read(imgOriginal); // 从视频中读取一帧存入矩阵imgOriginal，并检验是否为空
51. if (!bSuccess) //imgOriginal为空，跳出循环
52. {
53. cout << "Cannot read a frame from video stream" << endl;
54. break;
55. }
57. /*******************选取目标，与背景分离********************/
58. Mat imgHSV;
59. vector<Mat> hsvSplit;
60. cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV); //取视频的一帧 从BGR 转换到HSV
62. //因为我们读取的是彩色图，直方图均衡化需要在HSV空间做
63. split(imgHSV, hsvSplit);
64. equalizeHist(hsvSplit[2],hsvSplit[2]);
65. merge(hsvSplit,imgHSV);
66. Mat imgThresholded;
68. //检查imgHSV每一个像素，在Scalar(iLowH, iLowS, iLowV)和Scalar(iHighH, iHighS, iHighV)之间就设置为255，并保存在 imgThresholded图像中，否则为0。
69. inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded);
71. //开操作 (去除一些噪点)
72. Mat element = getStructuringElement(MORPH_RECT, Size(5, 5));
73. morphologyEx(imgThresholded, imgThresholded, MORPH_OPEN, element);
74. //闭操作 (连接一些连通域，消除一些空洞)
75. morphologyEx(imgThresholded, imgThresholded, MORPH_CLOSE, element);
76. /*************************************************************/
78. int sum=0;
79. for( int i = 0; i < imgThresholded.rows; ++i)
80. {
81. //获取第 i 行首像素指针
82. uchar * p = imgThresholded.ptr<uchar>(i);
83. //对第 i 行的每个像素(byte)操作
84. for( int j = 0; j < imgThresholded.cols; ++j )
85. {
86. sum+=p[j];
87. }
88. }
90. if(sum>0) //判断imgThresholded是否为空
91. {
92. /*********************多目标的选取*****************************/
93. //定义轮廓和层次结构
94. vector<vector<Point> > contours;
95. vector<Vec4i> hierarchy;
96. //查找轮廓
97. findContours( imgThresholded, contours, hierarchy,RETR_EXTERNAL, CHAIN_APPROX_NONE );
98. int area, index = 0;
99. int maxIndex=0;
100. int maxArea = 10;
101. //遍历所有轮廓，判断最大轮廓
102. for( ; index >= 0; index = hierarchy[index][0] )
103. {
104. area = fabs(contourArea(contours[index]));
105. if(area > maxArea)
106. {
107. maxIndex=index;
108. maxArea = area;
109. //printf("max area == %i\n", maxArea);
110. }
111. Scalar color(255, 255, 255 );
112. drawContours(imgThresholded, contours, index, color, CV_FILLED, 8, hierarchy );//对轮廓内部着色
113. }
114. /*************************************************************/
116. // writer << imgThresholded;
117. /*********************圈定敌方的位置****************************/
118. vector<Point> points;//点值
119. points=contours[maxIndex];
121. //对给定的 2D 点集，寻找最小面积的包围矩形
122. Point2f vertex[4];
123. RotatedRect box = minAreaRect(Mat(points));
124. box.points(vertex);
125. /******目标像素位置******/
126. Point2f target=(vertex[0]+vertex[2])*0.5;
128. //己方到目标的向量
129. line(imgOriginal, (vertex[0]+vertex[2])*0.5,Point(IMAGE_WIDTH*0.5,IMAGE_HEIGHT), Scalar(0, 255, 0), 2, CV_AA);
131. /****************坐标转换*********************/
132. Mat intrinsic_matrix=(Mat_<float>(3,3) << 797.64461,0, 341.87424,0,797.70292,282.35509, 0, 0, 1);
133. Mat rotation_tanslation_matrix=(Mat_<float>(3,3) << 0.021766,0.999506,-88.259736,-0.009815,0.022903,144.411487,-0.999715,0.021537,860.462175);
134. Mat fundamental_matrix= intrinsic_matrix*rotation_tanslation_matrix;
135. fundamental_matrix=fundamental_matrix.inv();
137. Mat target3D(3,1,CV_8UC1);
138. Mat target2D=(Mat_<float>(3,1) <<target.x,target.y,1 );
139. target3D= fundamental_matrix*target2D;
141. //目标世界坐标
142. float X = target3D.at<float>(0,0)/target3D.at<float>(2,0);
143. float Y = target3D.at<float>(1,0)/target3D.at<float>(2,0);
145. // //显示世界坐标
146. // char XText[128],YText[128];
147. // snprintf(XText, sizeof(XText), "%f", X);
148. // snprintf(YText, sizeof(YText), "%f", Y);
149. // putText(imgOriginal,XText,Point(100,90),FONT_HERSHEY_SCRIPT_SIMPLEX, 1,Scalar(0,0,255), 3, 8);
150. // putText(imgOriginal,YText,Point(400,90),FONT_HERSHEY_SCRIPT_SIMPLEX, 1,Scalar(0,0,255), 3, 8);
152. Point2f cameraPos=Point(950,150);//相机世界坐标
153. /******************************************/
155. target=target-Point2f(IMAGE_WIDTH*0.5,IMAGE_HEIGHT);
156. float angle=-atan(target.x/target.y)*180/pi;
157. float distance=sqrt((cameraPos.x-X)*(cameraPos.x-X)+(cameraPos.y-Y)*(cameraPos.y-Y));
158. char angleText[128],distanceText[128];
159. snprintf(angleText, sizeof(angleText), "%f", angle);
160. snprintf(distanceText, sizeof(distanceText), "%f", distance);
162. //将数字绘到画面上
163. putText(imgOriginal,angleText,Point(100,30),FONT_HERSHEY_SCRIPT_SIMPLEX, 1,Scalar(0,0,255), 3, 8);
164. putText(imgOriginal,distanceText,Point(400,30),FONT_HERSHEY_SCRIPT_SIMPLEX, 1,Scalar(0,0,255), 3, 8);
166. //绘制出最小面积的包围矩形
167. for( int i = 0; i < 4; i++ )
168. line(imgOriginal, vertex[i], vertex[(i+1)%4], Scalar(100, 200, 211), 2, CV_AA);
170. /*************************************************/
171. }
172. else
173. {
174. putText(imgOriginal,"Looking for the enemy.......",Point(90,90),FONT_HERSHEY_PLAIN, 2,Scalar(0,0,255), 2, 8);
175. }
176. imshow("Original", imgOriginal); //原始图片
177. imshow("Thresholded Image", imgThresholded); //二值化图片
180. char key = (char) waitKey(50);
181. if(key == 27) //esc键退出
182. break;
183. }
184. return 0;
185. }