Hệ thống định vị 3d dùng stereo camera

ĐẠI HỌC QUỐC GIA TP HCM TRƢỜNG ĐẠI HỌC BÁCH KHOA ĐẶNG TÂM TRUNG HỆ THỐNG ĐỊNH VỊ 3D DÙNG STEREO CAMERA Chuyên ngành: TỰ ĐỘNG HÓA Mã số: 605260 LUẬN VĂN THẠC SĨ TP HỒ CHÍ MINH, THÁNG NĂM 2013 CƠNG TRÌNH ĐƢỢC HỒN THÀNH TẠI TRƢỜNG ĐẠI HỌC BÁCH KHOA – ĐHQG – HCM Cán hƣớng dẫn khoa học : TS NGUYỄN VĨNH HẢO (Ghi rõ họ, tên, học hàm, học vị chữ ký) Cán chấm nhận xét : TS HUỲNH THÁI HOÀNG (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 TRỌNG TÀI (Ghi rõ họ, tên, học hàm, học vị chữ ký) Luận văn thạc sĩ đƣợc bảo vệ Trƣờng Đại Học Bách Khoa, ĐHQG TP.HCM, ngày tháng năm 2013 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ĩ) Ts Nguyễn Đức Thành…………………………… Ts Trƣơng Đình Châu…………………………… Ts Huỳnh Thái Hoàng…………………………… Ts Nguyễn Trọng Tài…………………………… PGS Ts Nguyễn Thị Phƣơng Hà………………… Xác nhận Chủ tịch Hội đồng đánh giá luận văn Trƣởng khoa quản lý chuyên ngành sau luận văn đƣợc sửa chữa (nếu có) CHỦ TỊCH HỘI ĐỒNG TRƢỞNG KHOA QLCN TRƢỜNG ĐẠI HỌC BÁCH KHOA CỘNG HOÀ XÃ HỘI CHỦ NGHIÃ VIỆT NAM KHOA ĐIỆN - ĐIỆN TỬ Độc Lập - Tự Do - Hạnh Phúc -o0o Tp HCM, ngày tháng năm NHIỆM VỤ LUẬN VĂN THẠC SĨ Họ tên học viên: ĐẶNG TÂM TRUNG Ngày, tháng, năm sinh: 03/6/1985 Phái: Nam Nơi sinh: Bình Định Chuyên ngành: TỰ ĐỘNG HÓA MSHV: 10150058 1- TÊN ĐỀ TÀI: HỆ THỐNG ĐỊNH VỊ 3D DÙNG STEREO CAMERA 2- NHIỆM VỤ LUẬN VĂN: - Thiết kế phần cứng mơ hình xe điều khiển tay - Viết driver kết nối máy tính với stereo camera Bumblebee dùng thƣ viện Triclop phần mềm visual studio 2010 - Viết thuật tốn tìm tính tốn điểm đặc trƣng dùng visual studio 2010 - Viết chƣơng trình thực thuật tốn ƣớc lƣợng thông số định vị ba chiều x, y, z góc roll, pitch, yaw dựa vào điểm đặc trƣng dùng visual studio 2010 - Thực liên kết hàm xử lí chƣơng trình visual studio 2010 với matlab 2012b 3- NGÀY GIAO NHIỆM VỤ: (Ghi theo QĐ giao đề tài) 06/02/2012 4- NGÀY HOÀN THÀNH NHIỆM VỤ: (Ghi theo QĐ giao đề tài) 30/11/2012 5- HỌ VÀ TÊN CÁN BỘ HƢỚNG DẪN: TS NGUYỄN VĨNH HẢO Nội dung đề cƣơng Luận văn đƣợc hội đồng chuyên ngành thông qua C.BỘ HƢỚNG DẪN (Họ tên chữ ký) CHỦ NHIỆM BỘ MÔN (Họ tên chữ ký) KHOA QL CH.NGÀNH (Họ tên chữ ký) LỜI CẢM ƠN Trƣớc hết xin cảm ơn ba mẹ, gia đình ln ủng hộ nguồn động viên lớn, chỗ dựa vững tơi Kính gửi đến thầy TS NGUYỄN VĨNH HẢO lời cảm ơn chân thành sâu sắc, cảm ơn thầy tận tình hƣớng dẫn suốt trình thực luận văn Thạc sĩ Xin cảm ơn quý thầy cô môn Điều Khiển Tự Động khoa Điện - Điện Tử tận tình giảng dạy, trang bị cho chúng em kiến thức bổ ích khoảng thời gian học cao học Tôi xin cảm ơn tất ngƣời thân, bạn bè, đặc biệt nhóm học viên cao học khóa 2010 động viên, góp ý, giúp đỡ tơi nhiều suốt trình học tập thực luận văn TP Hồ Chí Minh, tháng năm 2013 Đặng Tâm Trung TÓM TẮT NỘI DUNG LUẬN VĂN THẠC SĨ Nội dung đề tài nghiên cứu hệ thống xử lý ảnh 3D tốc độ cao (hi-speed stereo camera) để góp phần tạo hệ thống định vị 3D có độ xác cao Hệ thống đƣợc gắn vào đối tƣợng di động có chức xác định góc nghiêng 3D vị trí 3D đối tƣợng theo phƣơng x, y, z Các bƣớc thực mục tiêu đề tài bao gồm:  Đọc ảnh stereo đồng thời từ camera dùng thƣ viện Triclop visual studio 2010: độ phân giải 640 x 480 pixels tốc độ khoảng khung hình giây  Thực điều ch nh độ sáng, cân trắng, độ m o dạng ảnh thu đƣợc  Phát điểm đặc trƣng ảnh tìm mối tƣơng quan điểm đặc trƣng ảnh hai cặp ảnh liên tiếp  Lập trình thuật tốn Gauss-Newton, Ransac để ƣớc lƣợng tính tốn khoảng dịch chuyển camera ( tốc độ khoảng km/h ) thông qua thông số sau với sai số khoảng 10%:  Tọa độ thiết bị theo trục X,Y,Z  Giá trị góc Pitch,Yaw,Roll  Tổng khoảng cách di chuyển  Vẽ lại quãng đƣờng đƣợc hiển thị thông số tính tốn đƣợc camera matlab Abstract A common prerequisite for many vision-based driver assistance systems is the knowledge of the vehicle’s own movement In this paper we propose a novel approach for estimating the egomotion and coordinates of the vehicle from a sequence of stereo images The method of thesis is directly based on the GaussNewton and Ransac algorithm, thus no time expensive recovery of the 3dimensional scene structure is needed The only assumption we make is known camera geometry, where the calibration may also vary over time This thesis employs a Gauss-Newton algorithm in combination with a RANSAC-based outlier rejection scheme which yields robust frame-to-frame motion matrix (R, t) estimation even in dynamic environments Experiments show that thesis’s approach is clearly superior compared to other filtering techniques in terms of both, accuracy and run-time Luận Văn Thạc Sĩ – Hệ thống định vị 3D dùng Stereo Cameras Mục lục CHƢƠNG 1: TỔNG QUAN VỀ VISUAL ODOMETRY VÀ STEREO CAMERA 1.1 Khái niệm Visual Odometry 1.2 Stereo camera 1.3 Những ứng dụng Visual Odometry giới 1.3.1 Ứng dụng Mobile robot 1.3.2 Ứng dụng mơ hình máy bay trực thăng tự điều khiển 10 1.3.3 Visual Odometry sử dụng Stereo camera 11 1.4 Mục tiêu đề tài 14 1.5 Các bƣớc thực mục tiêu đề tài bao gồm 15 1.6 Những nội dung cần giải 15 1.7 Các báo cáo nghiên cứu khoa học liên quan 16 CHƢƠNG 2: CƠ SỞ LÝ THUYẾT CỦA ĐỀ TÀI 17 2.1 Các khái niệm xử lý ảnh 17 2.1.1 Phần tử ảnh (pixel) 17 2.1.2 Ảnh 17 2.1.3 Mức xám (gray level) 18 2.1.4 Ảnh đen trắng 18 2.1.5 Ảnh màu 19 2.1.6 Các định dạng 22 2.2 Mô hình camera hiệu ch nh thơng số 23 2.2.1 Stereo camera Bumblebee 23 2.2.2 Mơ hình camera 24 2.2.3 Cơ hình học ph p chiếu 26 2.2.4 Sự biến dạng thấu kính 27 2.2.5 Ma trận xoay vector dịch 30 2.2.5.1 Ma trận xoay 30 2.2.5.2 Vector dịch 32 2.2.6 Hiệu ch nh ảnh thu đƣợc 32 2.2.7 Thông số bên camera 34 Trang Luận Văn Thạc Sĩ – Hệ thống định vị 3D dùng Stereo Cameras 2.2.7.1 Các thông số nội camera 34 2.2.7.2 Thông số bên camera 34 2.3 Hình học ảnh 35 2.3.1 Mối quan hệ hệ trục tọa độ 35 2.3.2 Mối quan hệ tọa độ camera tọa độ mặt phẳng ảnh 36 2.4 Hệ tọa độ Homogeneous 37 2.5 Mối quan hệ tọa độ camera tọa độ chuẩn 37 2.6 Mối quan hệ tọa độ mặt phẳng ảnh tọa độ điểm ảnh 38 2.7 Lý thuyêt hình học 3D 39 2.8 Thƣ viện triclops 40 2.9 Phát góc 41 2.9.1 Các ứng dụng sử dụng điểm đặc trƣng điểm góc 41 2.9.2 Những yêu cầu phát góc 42 2.9.3 Các phƣơng pháp phát góc 43 2.9.4 Thuật tốn phát góc 44 2.10 Thuật toán Non-Maximum Suppression 45 2.10.1 NMS chiều 46 2.10.2 NMS chiều 50 2.11 Tổng sai lệch tuyệt đối 52 CHƢƠNG 3: VISUAL ODOMETRY SỬ DỤNG STEREO CAMERA 53 3.1 Thuật toán Visual Odometry 53 3.2 Dùng thƣ viện Triclops bắt ảnh từ stereo camera Bumblebee2 53 3.3 Chƣơng trình đọc thông số camera Bumblebee2 55 3.4 Hiệu ch nh ảnh thu đƣợc (Rectified) 57 3.5 Tìm điểm đặc trƣng góc blob (Feature detection) 58 3.6 Thuật toán Non-Maximum Suppression 59 3.7 Tìm điểm đặc trƣng giống hai cặp ảnh (Feature matching) 60 3.7.1 Epipolar Geometry 61 3.7.2 Epipolar Constraint (trƣờng hợp có Calibrated) 62 3.8 Tính tốn Disparity xây dựng điểm ảnh 3D 63 Trang Luận Văn Thạc Sĩ – Hệ thống định vị 3D dùng Stereo Cameras 3.9 Ƣớc lƣợng chuyển động 64 3.9.1 Kỹ thuật Bucketing 64 3.9.2 Thuật toán Gauss-Newton 65 3.9.3 Thuật toán RANSAC 67 CHƢƠNG 4: KẾT QUẢ THỰC NGHIỆM MƠ HÌNH 71 4.1 Mơ hình phần cứng 71 4.2 Kết phƣơng pháp matching tracking điểm đặc trƣng 72 4.3 Kết thực Visual Odometry môi trƣờng khác 76 4.3.1 Môi trƣờng nhà 76 4.3.2 Mơi trƣờng ngồi trời 91 CHƢƠNG 5: KẾT LUẬN VÀ HƢỚNG PHÁT TRIỂN 97 5.1 Các kết đạt đƣợc 97 5.2 Một số hạn chế 97 5.3 Hƣớng phát triển đề tài 98 Tài liệu tham khảo 99 PHẦN LÝ LỊCH TRÍCH NGANG: 101 Phụ lục A: Code chƣơng trình Visual Odometry Matlab 102 Phụ lục B: Code driver đọc ảnh từ Stereo camera Bumblebee 105 Phụ lục C: Chƣơng trình tìm tính tốn điểm đặc trƣng…………………………… 107 Phụ lục D: Chƣơng trình tính toán ƣớc lƣợng chuyển động ……………………… 118 Trang Luận Văn Thạc Sĩ – Hệ thống định vị 3D dùng Stereo Cameras CHƢƠNG 1: GIỚI THIỆU TỔNG QUAN VỀ VISUAL ODOMETRY VÀ STEREO CAMERA 1.1 Khái niệm Visual Odometry: Ƣớc lƣợng chuyển động camera đặc biệt stereo camera nhiệm vụ quan trọng lĩnh vực rô bốt hệ thống trợ giúp cho lái xe Nó cịn đƣợc ứng dụng nhiều hệ thống khác nhƣ hệ thống tự hành, định vị xác định vị trí… Thơng thƣờng việc xác định vị trí dựa vào cảm biến đo tốc độ bánh xe hay thiết bị cảm biến IMUs Odemetry phƣơng pháp xác định vị trí hƣớng chuyển động thiết bị cách đo vòng xoay bánh xe thiết bị Kỹ thuật odometry truyền thống đƣợc ứng dụng nhiều thiết bị có bánh xe đƣờng ray Odometry khơng cịn đƣợc sử dụng phƣơng pháp điều khiển chuyển động không theo chuẩn ví dụ nhƣ rơ bốt chân Phƣơng pháp odometry truyền thống gặp vấn đề nhƣ sau:  Sự xác phụ thuộc vào độ xác thiết bị đo vịng xoay bánh xe rơ bốt  Bề mặt mà rô bốt di chuyển phải phẳng  Bánh xe bị trƣợt di chuyển sàn  Sai số vị trí đƣợc tích lũy Trong năm gần hệ thống camera trở nên rẻ hơn, nhỏ tốc độ xử lí nhanh Chính điều giúp ta bắt đƣợc ảnh với tốc độ cao xử lí ảnh thời gian thực Việc ƣớc lƣợng xác chuyển động camera dựa vào cặp ảnh liên tiếp thu đƣợc gọi Visual Odometry Để khắc phục nhƣợc điểm ta sử dụng Visual Odometry Visual Odometry phƣơng pháp dùng để xác định vị trí hƣớng chuyển động rơ bốt cách phân tích hình ảnh thu đƣợc liên tiếp từ camera Visual odometry giúp tăng độ xác chuyển động rô bốt nhƣ thiết Trang Luận Văn Thạc Sĩ – Hệ thống định vị 3D dùng Stereo Cameras //void Matcher::nonMaximumSuppression (int16_t* I_f1,int16_t* I_f2,const int32_t* dims,vector &maxima,int32_t nms_n) { void Matcher::nonMaximumSuppression (int16_t* I_f2,const int32_t* dims,vector &maxima,int32_t nms_n) { // extract parameters int32_t width = dims[0]; int32_t height = dims[1]; int32_t bpl = dims[2]; int32_t n = nms_n; int32_t tau = param.nms_tau; // loop variables /*int32_t f1mini,f1minj,f1maxi,f1maxj,f2mini,f2minj,f2maxi,f2maxj; int32_t f1minval,f1maxval,f2minval,f2maxval,currval;*/ //int32_t f2mini,f2minj,f2maxi,f2maxj; int32_t f2maxi,f2maxj; int32_t f2maxval,currval; int32_t addr; /*for (int32_t i=n+margin; id3; *(max2+k++) = it>d4; *(max2+k++) = it->d5; *(max2+k++) = it->d6; *(max2+k++) = it->d7; *(max2+k++) = it>d8; } }*/ } void Matcher::createIndexVector (int32_t* m,int32_t n,vector *k,const int32_t &u_bin_num,const int32_t &v_bin_num) { // descriptor step size int32_t step_size = sizeof(Matcher::maximum)/sizeof(int32_t); // for all points for (int32_t i=0; i constrain to 1d if (!flow) { v_min = v1-param.match_disp_tolerance; v_max = v1+param.match_disp_tolerance; } // bins of interest int32_t u_bin_min = min(max((int32_t)floor(u_min/(float)param.match_binsize),0),u_bin_num1); Trang 115 Luận Văn Thạc Sĩ – Hệ thống định vị 3D dùng Stereo Cameras int32_t u_bin_max = min(max((int32_t)floor(u_max/(float)param.match_binsize),0),u_bin_num1); int32_t v_bin_min = min(max((int32_t)floor(v_min/(float)param.match_binsize),0),v_bin_num1); int32_t v_bin_max = min(max((int32_t)floor(v_max/(float)param.match_binsize),0),v_bin_num1); // for all bins of interest for (int32_t u_bin=u_bin_min; u_bin