Ứng dụng xử lý ảnh trong điều khiển chuyển động theo hướng của robot cá 3 khớp

ĐẠI HỌC QUỐC GIA TP HCM TRƯỜNG ĐẠI HỌC BÁCH KHOA HOÀNG QUỐC PHƯƠNG ỨNG DỤNG XỬ LÝ ẢNH TRONG ĐIỀU KHIỂN CHUYỂN ĐỘNG THEO HƯỚNG CỦA ROBOT CÁ KHỚP Chuyên ngành: Kỹ Thuật Cơ Điện Tử Mã số: 10390704 LUẬN VĂN THẠC SĨ TP Hồ Chí Minh,tháng 07 năm 2014 CƠNG TRÌNH ĐƯỢC HỒN THÀNH TẠI TRƯỜNG ĐẠI HỌC BÁCH KHOA –ĐHQG TPHCM Cán hướng dẫn khoa học : TS Võ Tường Quân (Ghi rõ họ, tên, học hàm, học vị chữ ký) Cán chấm nhận xét : TS Nguyễn Quốc Chí (Ghi rõ họ, tên, học hàm, học vị chữ ký) Cán chấm nhận xét : TS Phạm Công Bằng (Ghi rõ họ, tên, học hàm, học vị chữ ký) Luận văn thạc sĩ bảo vệ Trường Đại học Bách Khoa, ĐHQG Tp HCM ngày 15 tháng 07 năm 2014 Thành phần Hội đồng đánh giá luận văn thạc sĩ gồm: (Ghi rõ họ, tên, học hàm, học vị Hội đồng chấm bảo vệ luận văn thạc sĩ) Chủ tịch: PGS.TS Nguyễn Tấn Tiến Thư ký: TS Phùng Trí Cơng Ủy viên: TS Nguyễn Quốc Chí Ủy viên: TS Phạm Cơng Bằng Ủy viên: Võ Hoàng Duy Xác nhận Chủ tịch Hội đồng đánh giá LV Trưởng Khoa quản lý chuyên ngành sau luận văn sửa chữa (nếu có) CHỦ TỊCH HỘI ĐỒNG TRƯỞNG KHOA CƠ KHÍ ĐẠI HỌC QUỐC GIA TP.HCM TRƯỜNG ĐẠI HỌC BÁCH KHOA CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập - Tự - Hạnh phúc NHIỆM VỤ LUẬN VĂN THẠC SĨ Họ tên học viên: Hoàng Quốc Phương MSHV: 10390704 Ngày, tháng, năm sinh: 30 / 01 / 1985 Nơi sinh: Đồng Nai Chuyên ngành: Kỹ thuật Cơ Điện Tử Mã số : 605268 I TÊN ĐỀ TÀI: ỨNG DỤNG XỬ LÝ ẢNH TRONG ĐIỀU KHIỂN CHUYỂN ĐỘNG THEO HƯỚNG CỦA ROBOT CÁ KHỚP II NHIỆM VỤ VÀ NỘI DUNG: - Ứng dụng xử lý ảnh việc xác định vị trí robot, vị trí điểm đích, hướng chuyển động - Ứng dụng kết khoảng cách, góc lệch robot so với đích đến toán để điều khiển robot cá di chuyển từ vị trí ban đầu đến vị trí đích - Thực nghiệm ứng dụng xử lý ảnh mơ hình robot cá sử dụng động RC servo với điều khiển dsPIC 30F6014A III NGÀY GIAO NHIỆM VỤ :20 / 01 /2014 IV NGÀY HOÀN THÀNH NHIỆM VỤ:20 / 06 / 2014 V CÁN BỘ HƯỚNG DẪN (Ghi rõ học hàm, học vị, họ, tên):TS Võ Tường Quân Tp HCM, ngày tháng năm 2014 CÁN BỘ HƯỚNG DẪN (Họ tên chữ ký) CHỦ NHIỆM BỘ MÔN ĐÀO TẠO (Họ tên chữ ký) TRƯỞNG KHOA CƠ KHÍ (Họ tên chữ ký) LỜI CẢM ƠN Để có kiến thức hôm nay, xin chân thành cảm ơn thầy môn Cơ Điện tử - Khoa Cơ Khí, Trường Đại học Bách Khoa, Thành phố Hồ Chí Minh Tôi xin chân thành cám ơn Tiến sĩ Võ Tường Quân - thầy hướng dẫn thực luận văn Thầy nhiệt tình hướng dẫn, trợ giúp động viên tơi nhiều suốt q trình thực luận văn Tôi học hỏi nhiều từ kiến thức phương pháp nghiên cứu nhiệt tâm người thầy với học trị Tơi xin cảm ơn tất bạn bè người thân giúp đỡ để tơi có ngày hơm Thành phố Hồ Chí Minh, ngày 20 tháng năm 2014 Hồng Quốc Phương i TĨM TẮT LUẬN VĂN Robot sinh hướng nghiên cứu ngày phát triển nhanh chóng Những nghiên cứu tập trung vào nhiều loại robot sinh học khác Một số loại tiếng robot sinh mô tả chuyển động bay, chạy bơi Nội dung luận văn tập trung nhiều vào robot nước Và robot sinh nối tiếng nghiên cứu khắp nơi giới robot cá Với ưu điểm vượt trội so với phương tiện di chuyển nước truyền thống: linh hoạt tiêu tốn lượng Mục đích nghiên cứu ứng dụng xử lý ảnh điều khiển chuyển động robot cá khớp dạng Carangiform ABSTRACT Biomimetic robots are new research area which is developing quickly in recently Those researches focus on many kinds of the biomimetic robots Some of the famous ones are the flying, running and swimming biomimetic robots The content of this thesis mainly pay more attention to the underwater robot And, one of the most popular underwater biomimetic robot that is studied by many researches all around the world is fish robot Flexibility and less energy consumption are great advantages than traditional underwater robot The purpose of this research is the applications of image processing technique to control the moving direction of a 3-joint Carangiform fish robot ii LỜI CAM ĐOAN CỦA TÁC GIẢ Tôi xin cam đoan nghiên cứu lý thuyết thực nghiệm tơi thực Nếu có chép bất hợp pháp nào, tơi xin chịu tồn trách nhiệm iii MỤC LỤC CHƢƠNG 1: TỔNG QUAN VẤN ĐỀ NGHIÊN CỨU 1.1 Tổng quan 1.2 Tình hình nghiên cứu trong, ngồi nƣớc 1.3 Tình hình nghiên cứu, ứng dụng xử lý ảnh 1.4 Hƣớng nghiên cứu đề tài CHƢƠNG 2: MƠ HÌNH HĨA 11 2.1 Giả thuyết ký hiệu 11 2.2 Mơ hình hóa 13 CHƢƠNG 3: XỬ LÝ ẢNH VÀ ỨNG DỤNG XỬ LÝ ẢNH XÁC ĐỊNH KHOẢNG CÁCH GIỮA ĐIỂM TRONG MẶT PHẲNG 17 3.1 Tổng quan xử lý ảnh 17 3.2 Một số khái niệm ảnh điểm ảnh 18 3.2.1 Điểm ảnh 18 3.2.2 Độ phân giải ảnh 18 3.2.3 Mức xám ảnh 18 3.3 Tìm hiểu mơ hình camera 19 3.3.1Các đặc tính camera 19 3.3.2Vấn đề ma trận camera 19 3.4 Camera Calibration – Hiệu chỉnh camera 21 3.4.1 Hiệu chỉnh camera mục đích hiệu chỉnh camera 21 3.4.2 Phƣơng pháp hiệu chỉnh camera 21 3.5 Tiến hành Calib camera kiểm tra độ xác kết calib 3.5.1 Tiến hành calib camera 25 25 iv 3.5.2 Ứng dụng kết calib xác định khoảng cách điểm màu cho trƣớc 28 CHƢƠNG 4: ỨNG DỤNG XỬ LÝ ẢNH TRONG ĐIỀU KHIỂN ROBOT CÁ 32 Quá trình xử lý ảnh 32 4.1.1 Chụp ảnh từ video 32 4.1.2 Chuyển ảnh xám 33 4.1.3 Nhận dạng theo màu riêng biệt 33 4.1.4 Tìm đƣờng bao 34 4.1 4.2 Ứng dụng xử lý ảnh điều khiển robot cá 37 4.2.1 Các chế độ quẹo robot cá 37 4.2.2 Xây dựng thuật toán bơi robot cá dựa vào kết xử lý ảnh 39 CHƢƠNG 5:KẾT QUẢ VÀ HƢỚNG PHÁT TRIỂN 44 5.1 Mơ hình robot cá 44 5.2 Kết thực nghiệm ứng dụng xử lý ảnh robot cá 46 5.2.1 Mục tiêu thực nghiệm: 46 5.2.2 Cách tiến hành thực nghiệm: 46 5.2.3 Kết thực nghiệm: 47 5.2.4 Nhận xét kết thực nghiệm: 50 5.3 Kết luận hƣớng phát triển 51 5.3.1 Kết luận: 51 5.3.2 Hƣớng phát triển: 51 TÀI LIỆU THAM KHẢO 53 PHỤ LỤC 55 v DANH SÁCH HÌNH VẼ Hình 1.1 Robot cá Đại học Sƣ Phạm Kỹ Thuật [2] Hình 1.2 Một số nghiên cứu robot cá dạng BCF [1] Hình 1.3 Robot cá National Maritimes Research Institute [ 3] Hình 1.4 Robot Tuna I – MIT USA [4 ] Hình 1.5 Robot Tuna II - MIT U.S.A [5] Hình 1.6 Robot cá Đại học Essen nƣớc Anh (G8) [6] Hình 1.7 Robot dạng mực đại học Osaka [1] Hình 1.8 Robot dạng knifefish [1] Hình 1.9 Bố trí sơ đồ điều khiển robot cá dùng giải thuật xử lý ảnh [7] Hình 1.10 Các robot cá đƣợc nhận dạng màu sắc bể nƣớc [7] Hình 1.11 Sơ đồ bố trí Camera hệ thống truyền nhận điều khiển robot cá [8] Hình 1.12 Sơ đồ mơ tả nhận dạng màu (a) Robot cá đƣợc gắn màu nhận dạng; (b) Sơ đồ mơ tả dấu màu [9] Hình 1.13 Kết nghiên cứu phân tích giải thuật [10] Hình 1.14 Quy ƣớc điểm màu dùng để xác định robot, điểm đích Hình 2.1 Mơ hình robot cá khâu, khớp.[13] 11 Hình 2.2 Sơ đồ động khâu robot cá.[13] 12 Hình 2.3 Mơ hình lực tác dụng lên robot cá[13] 12 Hình 2.4 Mộ hình lực thủy động phần đi[14] 14 Hình 2.5 Mơ hình vận tốc phần [14] 14 Hình 3.1 Quá trình xử lý ảnh 17 Hình 3.2 Các bƣớc hệ thống xử lý ảnh 17 Hình 3.3 Điểm ảnh ảnh số 18 vi Hình 3.4 Chuyển ảnh từ ảnh màu sang ảnh xám[15] 19 Hình 3.5 Mối quan hệ hệ tọa độ thực hệ tọa độ camera [18] 20 Hình 3.6 Mẫu dùng để calib camera (dạng bàn cờ 7x10) [19] 22 Hình 3.7 Webcam Logitech C270 (độ phân giải 1280 x 720) 25 Hình 3.8 Bố trí camera 26 Hình 3.9 Trƣớc calib 26 Hình 3.10 Hình ảnh vị trí mẫu q trình calib 27 Hình 3.11 Sau calib 27 Hình 3.12 File chứa thơng số nội thơng số ngoại camera 28 Hình 3.13 Khoảng cách điểm màu bố trí thực nghiệm 30 Hình 3.14 Kết đo camera 30 Hình 4.1 Webcam Logitech C270 32 Hình 4.2 Phƣơng pháp xác định đƣờng bao dựa theo màu 34 Hình 4.3 Qui ƣớc điểm màu không gian hoạt động robot 35 Hình 4.4 Lƣu đồ giải thuật trình điều khiển 36 Hình 4.5Chế độ quẹo robot cá[22] 37 Hình 4.6 Mode A chế độ quẹo robot cá 37 Hình 4.7 Mode B chế độ quẹo robot cá 38 Hình 4.8 Mode C chế độ quẹo robot cá 38 Hình 4.9 Mơ hình khớp robot cá 39 Hình 4.10 Ký hiệu góc lệch khoảng cách L 39 Hình 4.11 Các kiểu điều khiển robot cá 41 Hình 4.12 Cách xác định Lmin 41 Hình 4.13 Lƣu đồ giải thuật 43 vii Chƣơng 5: Kết hƣớng phát triển t=15s t=17s t=19s t=20s Hình 5.3 Hình dáng chuyển động robot 48 Chƣơng 5: Kết hƣớng phát triển b) Thực nghiệm 2: Ứng dụng xử lý ảnh điều khiển robot cá di chuyển đến đích trƣờng hợp điểm đích thả tự mơi trƣờng nƣớc t=3s t=1s t=5s t=7s t=9s t=11s Hình 5.4 Hình dáng chuyển động robot 49 Chƣơng 5: Kết hƣớng phát triển Hình 5.5 Đồ thị đáp ứng 5.2.4 Nhận xét kết thực nghiệm: Kết thực nghiệm cho thấy kết xử lý ảnh mơi trƣờng thực tế tốt, xác định đƣợc vị trí robot vị trí điểm đích, từ đƣa cách điều khiển hiệu Việc điều khiển robot di chuyển đến đích đạt kết tốt, đáp ứng đƣợc yêu cầu đề Do kích thƣớc hồ giới hạn nên việc tiến hành thực nghiệm hạn chế 50 Chƣơng 5: Kết hƣớng phát triển 5.3 Kết luận hƣớng phát triển 5.3.1 Kết luận: Luận văn giải đƣợc vấn đề: a) Đề xuất giải pháp thay phịng thí nghiệm cho cảm biến vị trí tốn điều khiển b) Xác định đƣợc thơng số điều khiển nhƣ thông số bên ( f X , f y , c X , c y ) thơng số bên ngồi (  ,  ,  , Tx , Ty , Tz ) từ thực nghiệm c) Thực nghiệm kiểm chứng giải pháp đề 5.3.2 Hƣớng phát triển: a) Nghiên cứu vây hơng nhằm cân ổn định vị trí cho robot b) Nghiên cứu, phát triển điều khiển phù hợp với yêu cầu điều khiển cao 51 Chƣơng 5: Kết hƣớng phát triển Kết đề tài nghiên cứu luận văn đƣợc viết báo đƣợc đăng lên kỷ yếu hội nghị: - Hội nghị toàn quốc lần thứ Điều khiển Tự động hóa – VCCA 2013 diễn từ 22-23/11/2013 Đà Nẵng Tên báo: A Study on Image Processing to Control Moving Directions of a 3-Joint Carangiform Fish Robot Quoc Phuong Hoang, 2Tuong Quan Vo Department of Mechatronics, Ho Chi Minh City, University of Technology – VietNam e-Mail: 1quocphuongspkt@gmail.com, 2vtquan@hcmut.edu.vn - Hội nghị quốc tế Cơ điện tử Robot, diễn từ 10-11/12/2013 trƣờng Đại học Bách Khoa TpHCM Tên báo: An Application of Image Processing to Control The Straight Motion of a 3-Joint Carangiform Fish Robot Thanh Lap Dang, Hoang Hiep Ly, Phu Huynh Duc, Thanh Phat Vo, Quoc Phuong Hoang, Tuong Quan Vo Faculty of Mechanical Engineering, HoChiMinh City University of Technology,VN e-Mail: vtquan@hcmut.edu.vn 52 Tài liệu tham khảo TÀI LIỆU THAM KHẢO [1] K H Low “Modelling and parametric study of modular undulating fin rays for fish robots, ” Mechanism and Machine J 44, 615-632 (2009) [2] Robot cá Việt Nam [3] Model Fish Robot, National Maritimes Research Institute < http://www.nmri.go.jp/eng/khirata/fish/model/ppf09/index_e.html> [4] Robot Tuna I – MIT USA < http://www.communistrobot.com/robotarticles.php?articleid=60 > [5] D Beal,M Sachinis, RoboTuna II,Massachusetts Institute of Technology http://web.mit.edu/towtank/www/Tuna/Tuna2/tuna2.html [6] Fish Robot of Essen Universiry (G8) [7] Junzhi Yu, Yimin Fang, Wei Zhao, and Long Wang, Control and Coordination of Biomimetic Robotic Fish, Center for Systems and Control, Department of Mechanics and Engineering Science, Peking University, Beijing 100871, P R China [8] Qin Yan, Zhen Han, Shi-wu Zhang, Jie Yang, Parametric Research of Experiments on a Carangiform RoboticFish, Department of Precision Machinery and Precision Instrumentation, University of Science and Technology of China, Hefei 230026, P R China [9] JunZhi Yu, Long Wang, Wei Zhao, Min Tan, Optimal design and motion control of biomimetic robot fish 53 Tài liệu tham khảo [10] Jin-Kui Chu, Rong- Hua Li, Qing-Ying Li, Hong-Qing Wang, A visual attention model for robot object tracking, 2009 [11] Phạm Văn Anh (tháng 07 năm 2011) “Nghiên cứu xây dựng điều khiển cho Robot cá di chuyển theo phương cho trước mặt phẳng 2D”, Luận văn thạc sĩ [12] Lê Vũ Tuấn Anh (2012) “Nghiên cứu, thiết kế robot cá dạng carangiform khớp động”, Luận văn đại học [13] Junzhi yu, Lizhong Liu, Long Wang, “Dynamic Modeling of Robotic Fish Using Schiehien's Method,” IEEE Int Conf on Robotics and Biomimetics, 457-462 (2006) [14] Motomu Nakashima, Norifumi Ohgishi and Kyosuke Ono, “A Study on the Propulsive Mechanism of a Double Jointed Fish Robot Utilizing Self-Excitation Control” JSME Int Journal Advances in Motion and Vibration Control Technology 46(3), 982-990 (2003) [15] http://opencv.willowgarage.com/documentation/c/index.html [16] Alexis M Martinez,Image Processing- CameraModels [17]Derek Hoiem,Projective Geometry and Camera Models, University of Illinois, 2011 [18] Gary Bradski, Adrian Kaebler “Learning OpenCV Computer Vision with The OpenCV Library” O’Reilly Media, United State of America, 2008 [19] OpenCV Chessboard,http://sourceforge.net/projects/opencvlibrary [20] Qi Zhao and Hai Tao, “Object tracking using Color Correlogram” Department of Computer Enginnering, University of California Santa Cruz, CA 95064 [21] HM-TR Transparent Wireless Data Link Module [22] K H Low “Modelling and parametric study of modular undulating fin rays for fish robots, ” Mechanism and Machine J 44, 615-632 (2009) 54 Phụ lục PHỤ LỤC Code xử lý ảnh (viết Visual C++ 2010) #include #include #include #include #include #include #include "Cricle.h" #include #include #include #include #include #include /////////////////////////////////////////////////////////////////////////// double fx=0.03872 , fy=0.02581 , cx=23.6080 , cy=17.6968; // thông số bên double xt=0.85433 , yt=0.58924 , zt=-1.62742 ; // vecto quay (t) double Tx=-24.38017 , Ty=-17.30255 , Tz=69.24069;// thông số tịnh tiến (T) double Xreal , Yreal ; // Tọa độ thực chuong trinh double R11, R12, R13, R21, R22, R23, R31, R32, R33; // ma trận xoay double m11, m12, m13, m14, m21, m22, m23, m24, m31, m32, m33, m34;// ma trận M double Ximg1 , Yimg1, Ximg2, Yimg2, Ximg3, Yimg3 ;// Tọa độ ảnh double a1 , b1 , c1 , a2 , b2 , c2 ; // Biến trung gian double angle1 , angle2; // Góc double d_1, d_2, d_3 , k1, k2, goc , R1, R2, R3, Point1X, Point2X, Point3X, Point1Y, Point2Y, Point3Y; // Thông số đƣờng thẳng nối start stop double PI=3.14159; char key; using namespace cv; //////////////////////////////////////////////Chuong trinh tinh ma tran int tinh_matran(double xt, double yt, double zt, double Tx, double Ty, double Tz, double fx, double fy, double cx, double cy ) { R11=cos(xt)*cos(yt); // Ma trân xoay R R12=cos(xt)*sin(yt)*sin(zt)-sin(xt)*cos(zt); R13=sin(xt)*sin(zt)+cos(xt)*sin(yt)*cos(zt); R21=sin(xt)*cos(yt); R22=cos(xt)*cos(zt)+sin(xt)*sin(yt)*sin(zt); R23=sin(xt)*sin(yt)*cos(zt)-cos(xt)*sin(zt); R31=-sin(yt); R32=cos(yt)*sin(zt); R33=cos(yt)*cos(zt); 55 Phụ lục /////////////////////// m11=fx*R11+cx*R31; m12=fx*R12+cx*R32; m13=fx*R13+cx*R33; m14=fx*Tx+cx*Tz; m21=fy*R21+cy*R31; m22=fy*R22+cy*R32; m23=fy*R23+cy*R33; m24=fy*Ty+cy*Tz; // Ma trận M m31=R31; m32=R32; m33=R33; m34=Tz; return (m11,m12,m13,m14,m21,m22,m23,m24,m31,m32,m33,m34); } ////////////////////////////////////Chƣơng trình tính tọa độ thực int tinh_toado(int Ximg, int Yimg) { a1 = m11 - Ximg * m31; b1 = m12 - Ximg * m32; c1 = Ximg * m34 - m14; a2 = m21 - Yimg * m31; b2 = m22 - Yimg * m32; c2 = Yimg * m34 - m24; Yreal = (c1*a2-c2*a1)/(b1*a2-b2*a1); // Tọa độ thực Y Xreal = (c1-b1*Yreal)/a1; // Tọa độ thực X return (Xreal , Yreal); } //cai dat Min va Max cho bo loc HSV //su dung Trackbar int H_MIN = 0; int H_MAX = 256; int S_MIN = 0; int S_MAX = 256; int V_MIN = 0; int V_MAX = 256; //cai dat rong cua frame hinh const int FRAME_WIDTH = 1080; const int FRAME_HEIGHT = 720; //so luong Object lon nhat co the detect duoc frame const int MAX_NUM_OBJECTS=50; //Min va Max xua Object detect duoc const int MIN_OBJECT_AREA = 5*5; 56 Phụ lục const int MAX_OBJECT_AREA = FRAME_HEIGHT*FRAME_WIDTH/1.5; //ten cua cac windown const string windowName = "Original Image"; const string windowName1 = "HSV Image"; const string windowName2 = "Thresholded Image"; const string windowName3 = "After Morphological Operations"; const string trackbarWindowName = "Trackbars"; void on_trackbar( int, void* ) { } string intToString(int number) { std::stringstream ss; ss 0) { int numObjects = hierarchy.size(); //if number of objects greater than MAX_NUM_OBJECTS we have a noisy if(numObjects= 0; index = hierarchy[index][0]) { Moments moment = moments((cv::Mat)contours[index]); double area = moment.m00; if(area>MIN_OBJECT_AREA){ Cricle green; green.setXPos(moment.m10/area); green.setYPos(moment.m01/area); greens.push_back(green); objectFound = true; }else objectFound = false; } //let user know you found an object if(objectFound ==true){ //draw object location on screen drawObject(greens,cameraFeed); } ///x,y } else putText(cameraFeed,"TOO MUCH NOISE! ADJUST FILTER",Point(0,50),1,2,Scalar(0,0,255),2); } } int main(int argc, char* argv[]) { tinh_matran(xt, yt, zt, Tx, Ty, Tz, fx, fy, cx, cy); //////////////////////////////////////////////////////Thiet lap cong COM HANDLE hSerial = CreateFile(L"COM5", GENERIC_READ | GENERIC_WRITE, 0, 0, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0); if (hSerial !=INVALID_HANDLE_VALUE) { printf("Port opened! \n"); DCB dcbSerialParams; 59 Phụ lục GetCommState(hSerial,&dcbSerialParams); dcbSerialParams.BaudRate = CBR_9600; dcbSerialParams.ByteSize = 8; dcbSerialParams.Parity = NOPARITY; dcbSerialParams.StopBits = ONESTOPBIT; SetCommState(hSerial, &dcbSerialParams); } else { if (GetLastError() == ERROR_FILE_NOT_FOUND) { printf("Serial port doesn't exist! \n"); } printf("Error while setting up serial port! \n"); } char outputChars[] = "c"; DWORD btsIO; ////////////////////////////////////////////////////////////////////// //if we would like to calibrate our filter values, set to true bool calibrationMode = true; //Matrix to store each frame of the webcam feed Mat cameraFeed; Mat threshold; Mat HSV; if(calibrationMode){ //tao bar cho HSV createTrackbars(); } //video capture object to acquire webcam feed VideoCapture capture; //lenh mo camera capture.open(1); //set chieu cao va rong cho frame anh capture.set(CV_CAP_PROP_FRAME_WIDTH,FRAME_WIDTH); capture.set(CV_CAP_PROP_FRAME_HEIGHT,FRAME_HEIGHT); //start an infinite loop where webcam feed is copied to cameraFeed //all of our operations will be performed within this loop while(1){ //store image to matrix capture.read(cameraFeed); //chuyen sang he mau HSV 60 Phụ lục cvtColor(cameraFeed,HSV,COLOR_BGR2HSV); if(calibrationMode==true){ //if in calibration mode, we track objects based on the HSV cvtColor(cameraFeed,HSV,COLOR_BGR2HSV); inRange(HSV,Scalar(H_MIN,S_MIN,V_MIN),Scalar(H_MAX,S_MAX,V_MAX),thr eshold); morphOps(threshold); imshow(windowName2,threshold); trackFilteredObject(threshold,HSV,cameraFeed); } /////////////////////////////////////////////////////////////////////////// tinh_toado(Ximg1, Yimg1); // Gọi chƣơng trình tính tọa độ thật Point1X=Xreal; Point1Y=Yreal; tinh_toado(Ximg2, Yimg2); Point2X=Xreal; Point2Y=Yreal; tinh_toado(Ximg3, Yimg3); Point3X=Xreal; Point3Y=Yreal; if ((Ximg1 !=Ximg3) && (Ximg1!=Ximg2)) { k1 = (Point1Y-Point3Y)/(Point1X-Point3X); k2 = (Point1Y-Point2Y)/(Point1X-Point2Y); angle1 = atan(k1); angle2 = atan(k2); if (angle1 < 0) angle1=180+angle1; if (angle2 < 0) angle2=180+angle2; } goc = abs(angle1-angle2)*180/PI; d_1= sqrt((Point1X-Point2X)*(Point1X-Point2X)+(Point1Y-Point2Y)*(Point1Y-Point2Y)); cv::line(cameraFeed, cv::Point(Ximg1,Yimg1), cv::Point(Ximg2, Yimg2), cv::Scalar(255,0,0),2); cv::line(cameraFeed, cv::Point(Ximg1,Yimg1), 61 Phụ lục cv::Point(Ximg3, Yimg3), cv::Scalar(255,0,0),2); std::cout