Xây dựng bản đồ 3d trong không gian chưa xác định sử dụng camera kinect

MỤC LỤC TRANG NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP i LỊCH TRÌNH THỰC HIỆN ĐỒ ÁN TỐT NGHIỆP ii LỜI CAM ĐOAN iv LỜI CẢM ƠN v MỤC LỤC vi LIỆT KÊ HÌNH VẼ ix LIỆT KÊ BẢNG xi TÓM TẮT xii Chương TỔNG QUAN 13 1.1 ĐẶT VẤN ĐỀ 13 1.2 MỤC TIÊU 14 1.3 NỘI DUNG NGHIÊN CỨU 15 1.4 GIỚI HẠN 15 1.5 BỐ CỤC 15 Chương CƠ SỞ LÝ THUYẾT 17 2.1 HỆ ĐIỀU HÀNH ROS – ROBOT OPERATING SYSTEM 17 2.1.1 Giới thiệu tổng quan ROS 17 2.1.2 Trao đổi liệu ROS 18 2.2 THUẬT TOÁN SIFT – SCALE INVARIAN FEATURE TRANSFORM 19 2.2.1 Xác định điểm đặc trưng ảnh 19 2.2.2 Mơ tả đặc tính điểm đặc trưng 23 2.2.3 Nhận dạng cặp điểm tương đồng hai ảnh 2D 24 2.3 TÍNH MA TRẬN HOMOGRAPHY BẰNG THUẬT TỐN RANSAC 25 2.3.1 Thuật toán RANSAC 25 vi 2.3.2 Giới thiệu ma trận Homography 26 2.3.3 Tính ma trận Homography thuật toán RANSAC 27 2.4 CÁC PHÉP BIẾN ĐỔI TRONG TỌA ĐỘ BA CHIỀU VÀ PHƯƠNG PHÁP GHÉP CÁC ĐÁM MÂY 3D 28 2.4.1 Phép tịnh tiến 29 2.4.2 Phép xoay 29 2.4.3 Ghép cặp điểm tương đồng hai đám mây 3D 31 2.4.4 Ghép cặp điểm tương đồng nhiều đám mây 3D 33 2.5 NGUYÊN LÍ ĐO ĐỘ SÂU CỦA CAMERA KINECT 34 2.5.1 Giới thiệu camera Kinect 34 2.5.2 Những thành phần Kinect 34 2.5.3 Thông số kỹ thuật camera Kinect 35 2.5.4 Nguyên lí đo độ sâu 36 2.6 CHUYỂN ĐỔI DỮ LIỆU ẢNH 2D THU ĐƯỢC TỪ CAMERA KINECT THÀNH ẢNH 3D 38 Chương THIẾT KẾ 40 3.1 SƠ ĐỒ KHỐI TỔNG QUÁT CỦA CHƯƠNG TRÌNH XỬ LÝ 40 3.2 SƠ ĐỒ KHỐI QUY TRÌNH XỬ LÝ CỦA CHƯƠNG TRÌNH VẼ BẢN ĐỒ 41 Chương THI CÔNG HỆ THỐNG 43 4.1 LƯU ĐỒ GIẢI THUẬT 43 4.2 KẾT QUẢ THI CÔNG 45 4.2.1 Thu thập liệu từ Kinect 45 4.2.2 Tìm điểm đặc trưng vật thể ảnh RGB dùng thuật tốn SIFT 48 4.2.3 Tìm cặp điểm đặc trưng tương đồng hai ảnh RGB 50 4.2.4 Xác định điểm đặc trưng hai đám mây 3D 51 4.2.5 Xác định cặp tương đồng hai đám mây 3D tương ứng với cặp điểm tương đồng hai ảnh RGB 51 vii 4.2.6 4.3 Ghép cặp điểm tương đồng đám mây 3D với 52 VIẾT TÀI LIỆU HƯỚNG DẪN SỬ DỤNG, THAO TÁC 55 Chương KẾT QUẢ - NHẬN XÉT - ĐÁNH GIÁ 56 5.1 KẾT QUẢ THỰC HIỆN 56 5.2 ĐÁNH GIÁ 59 Chương KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 66 6.1 KẾT LUẬN 66 6.2 HƯỚNG PHÁT TRIỂN 67 TÀI LIỆU THAM KHẢO 68 PHỤ LỤC 69 viii LIỆT KÊ HÌNH VẼ Hình Trang Hình 2.1 Cấu trúc trao đổi liệu ROS 18 Hình 2.2 Các phiên ảnh vật mẫu tương ứng với hệ số k σ 21 Hình 2.3 Các phiên ảnh sai khác Dx, y, kσ tương ứng với hệ số 𝑘 σ 22 Hình 2.4 Hai mươi sáu vị trí so sánh với vị trí X để tìm 22 Hình 2.5 16x16 điểm ảnh xung quanh điểm đặc trựng vector Gradient tương ứng cho điểm ảnh [6] 23 Hình 2.6 Các vector Gradient thống kê thành giá trị góc quay khác [6] 24 Hình 2.7 Phép chiếu Homography 27 Hình 2.8 Hệ toạ độ Decartes 𝑂𝑥𝑦𝑧 𝑂′𝑥′𝑦′𝑧′ 28 Hình 2.9 Hai đám mây nắm hai hệ trục toạ độ khác có điểm x tương đồng 31 Hình 2.10 Hai đám mây có điểm tương đồng y ghép với 32 Hình 2.11 Ghép cặp điểm tương đồng bốn đám mây lại với 33 Hình 2.12 Camera Kinect 34 Hình 2.13 Các thành phần Kinect 35 Hình 2.14 Lược đồ thể mối quan hệ độ sâu với độ sai lệch 36 Hình 2.15 Dữ liệu ảnh 2D thu từ camera Kinect 38 Hình 2.16 Camera lấy thông tin độ sâu điểm ảnh 38 Hình 2.17 Hệ trục toạ độ Decartes ảnh 3D 39 Hình 3.1 Sơ đồ khối tổng quát hệ thống vẽ đồ 3D 40 Hình 3.2 Sơ đồ mơ tả chi tiết chương trình vẽ đồ 3D 42 Hình 4.1 Lưu đồ giải thuật chương trình 44 Hình 4.2 Kết nối Kinect với máy tính qua cổng USB 45 Hình 4.3 Cấp nguồn cho Kinect 46 Hình 4.4 Kết nối phần cứng hồn chỉnh 46 Hình 4.5 Chạy lệnh hồn thành việc giao tiếp Kinect máy tính 47 Hình 4.6 Hai ảnh RGB chụp hai góc độ khác 48 Hình 4.7 Hai đám mây 3D tương ứng với hai ảnh RGB chụp Hình 4.6 48 Hình 4.8 Điểm đặc trưng ảnh RGB thứ 49 ix Hình 4.9 Điểm đặc trưng ảnh RGB thứ hai 49 Hình 4.10 Các điểm đặc trưng hai ảnh RGB 50 Hình 4.11 Kết nối cặp điểm tương đồng hai ảnh RGB 50 Hình 4.12 Các điểm đặc trưng đám mây 3D 51 Hình 4.13 Kết nối cặp điểm tương đồng hai đám mây 3D 52 Hình 4.14 Ma trận chuyển đổi tính toán 53 Hình 4.15 Hai đám mây 3D kết nối lại với 53 Hình 4.16 Khơng gian 3D lối phịng học (ở góc nhìn thứ nhất) 54 Hình 4.17 Khơng gian 3D lối phịng học (ở góc nhìn thứ hai) 54 Hình 5.1 Khơng gian 3D hồn chỉnh phịng học (ở góc nhì thứ nhất) 56 Hình 5.2 Khơng gian 3D hồn chỉnh phịng học (ở góc nhì thứ hai) 57 Hình 5.3 Khơng gian 3D hồn chỉnh phịng học (ở góc nhì thứ ba) 57 Hình 5.4 Khơng gian 3D phịng học xoay Kinect quanh vị trí cố định (góc nhìn thứ nhất) 58 Hình 5.5 Khơng gian 3D phịng học xoay Kinect quanh vị trí cố định (góc nhìn thứ hai) 58 Hình 5.6 Biểu đồ thể sai số khoảng cách di chuyển Kinect tịnh tiến 60 Hình 5.7 Biểu đồ thể sai số góc quay xoay Kinect 61 Hình 5.8 Biểu đồ thể sai số khoảng cách đo thực tế tính tốn từ Kinect 62 Hình 5.9 Đám mây không gian 3D lối tự nhiên 63 Hình 5.10 Ảnh 2D lối tự nhiên (không xếp chi tiết) 63 Hình 5.11 Ảnh 2D lối xếp có nhiều chi tiết 64 Hình 5.12 Biểu đồ thống kê chênh lệch số lượng điểm đặc trưng khung hình chụp 65 x LIỆT KÊ BẢNG Bảng Trang Bảng 2.1: Bảng thống kê tầm nhìn camera Kinect 36 Bảng 5.1 Bảng thống kê sai số khoảng cách di chuyển tịnh tiến Kinect 59 Bảng 5.2 Bảng thống kê sai số góc quay xoay Kinect 60 Bảng 5.3 Bảng thống kê sai số khoảng cách đo từ Kinect đến đối tượng 62 Bảng 5.4 Bảng thống kê số lượng điểm đặc trưng 64 xi TÓM TẮT Đề tài thực việc xây dựng đồ 3D phịng học Để hồn thành mục đích đề tài việc cần camera thu thập liệu ảnh RGB ảnh độ sâu khung cảnh, đề tài nhóm sử dụng camera Kinect cho việc thu thập liệu từ hai liệu ta chuyển thành liệu đám mây điểm 3D khung cảnh Sau đó, nhóm sử dụng thuật tốn SIFT hai góc khác khung cảnh để tìm cặp điểm tương đồng hai ảnh RGB Tiếp theo, xác định cặp điểm tương đồng hai đám mây điểm 3D tương ứng với hai ảnh RGB Cuối cùng, xác định ma trận chuyển đổi để ghép cặp điểm tương đồng lại với nhau, đồng nghĩa với việc ghép hai đám mây 3D hai khung cảnh lại thành đám mây lớn tương ứng với khung cảnh lớn Tương tự chụp nhiều góc khung cảnh phịng thực ghép nhiều đám mây 3D lại với tạo thành đồ 3D phịng học Đám mây 3D phòng học sau tạo thành tương đối rõ ràng giống với không gian thực tế phịng học Bên cạnh đó, việc xây dựng đồ 3D có vai trị quan trọng lĩnh vực robot tự hành, cung cấp cho robot đồ 3D khơng gian mà di chuyển để robot định vị di chuyển cho phù hợp xii CHƯƠNG TỔNG QUAN Chương TỔNG QUAN 1.1 ĐẶT VẤN ĐỀ Bản đồ 3D ứng dụng rộng rãi Robot tự hành đóng vai trò quan trọng việc điều hướng, thao tác, điều khiển từ xa Robot Sau tìm tịi nghiên cứu nhóm cảm thấy thích thú với lĩnh vực nên định thực đồ án mô lại không gian 3D sử dụng camera Kinect Camera Kinect trả liệu ảnh RGB ảnh độ sâu không gian - ảnh 3D Với ảnh RGB thu được, nhóm sử dụng thuật tốn SIFT để xác định cặp điểm đặc trưng tương đồng hai khung ảnh chụp liên tiếp Sau đó, nhóm đối chiếu tọa độ cặp điểm đặc trưng tương đồng ảnh RGB lên ảnh cặp ảnh 3D tương ứng Dựa vào cặp điểm tương đồng đó, nhóm tính tốn để suy ma trận chuyển đổi với mục đích đồng hóa gốc tọa độ đám mây thứ hai với đám mây thứ thực ghép ảnh hai đám mây Đám mây sau ghép tiếp tục so sánh với đám mây Cứ tiếp tục thực việc ghép ảnh vậy, kết thu đám mây ngày hồn thiện Để hồn thành đồ án này, nhóm tham khảo qua báo có liên quan giới lĩnh xây dựng đồ 3D không gian sử dụng camera Kinect Trong báo “ 2D Mapping of a Closed Area by a Range Sensor ” nhóm tác giả Shinya OGAWA, Kajiro WATANABE, Kazuyuki KOBAYASHI Với mục đích muốn điều khiển xe không người lái khơng gian kín nhà kho,xưởng, phịng, Việc có đồ định vị cho xe cần thiết Nhóm sử dụng laser rangefinder để xác định phân ngưỡng vật thể kết hợp với vị trí hướng robot để xây dựng nên đồ 2D Với đồ thu sử dụng cho robot tự hành khơng gian kín giá trị trả có tọa độ x,y nên có nhiều bất lợi tháo tác điều khiển nâng cao [1] Trong báo “ 3D Mapping With a RGB-D Camera ” ca nhúm tỏc gi Felix Endres, Jurgen Hess, J ă urgen Sturm, Daniel Cremers, and Wolfram Burgard trình bày phương BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP 13 CHƯƠNG TỔNG QUAN pháp định vị vẽ đồ sử dụng camera RGB-D Kinect Asus Xtion Pro Live Bằng cách sử dụng thuật toán ICP (iterative-closest-point) phương pháp RANSAC (Random Sample Consensus) để xử lý liệu thu từ camera xây dựng lại đồ Điểm đặc biệt hệ thống thực thi trường hợp khác nghiệt camera di chuyển với tốc độ cao, môi trường điểm đặc trưng nhiều môi trường khác [2] Với báo “ 3D map reconstruction with sensor Kinect ” nhóm tác giả Peter Beňo, František Duchoň, Michal Tölgyessy, Peter Hubinský, Martin Kajan đến từ trường đại học Slovak Nhóm khảo sát việc xây dựng đồ theo hai cách sử dựng thuật toán RGBD-6D-SLAM Truncated signed distance function (TSDF) Điểm hạn chế đề tài cần phải có máy tính có cấu hình mạnh để xử lý thuật tốn cho kết xác [3] Ngồi ra, với báo “ Indoor Mapping using Kinect and ROS ” nhóm tác giả Hesham Ibrahim, Mohamed Ahmed Omara, Khairul Salleh Mohamed Sahari đến từ đại học Tenaga Nasional Malaysia trình bày việc lấy liệu từ camera Kinect tảng ROS (Robot Operating System) Tuy nhiên báo chưa chắn độ tin cậy camera Kinect, phương pháp thực điều kiện phòng trơn, nhẵn, thiếu ánh sáng [4] Bằng việc tham khảo kết hợp đề tài nghiên cứu có trước nhiều tác giả giới, nhóm cải tiến sử dụng camera giá rẻ Kinect hãng Mircosoft kết trả tương đối xác để thu thập liệu từ mơi trường mà cụ thể tồn khơng gian nhà Sau truyền liệu xuống camera tảng ROS sử dụng thuật toán SIFT, phương pháp chuyển tọa độ ma trận 3D, nhằm khôi phục lại không gian theo ba chiều Kết ảnh cho tương đối xác khoảng 60%, tốc độ xử lý không lâu 1.2 MỤC TIÊU Mục tiêu đề tài hoàn thành việc xây dựng đồ khơng gian 3D phịng học Bản đồ sau xây dựng không gian ba chiều gồm đám mây ghép lại Mỗi đám mây dùng để xây dựng đồ 3D chuyển đổi từ ảnh 2D BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP 14 CHƯƠNG TỔNG QUAN chụp từ camera Kinect Sau xử lí liệu thuật tốn, đồ 3D phịng học hoàn thành 1.3 1.4 NỘI DUNG NGHIÊN CỨU 1- Tìm hiểu ROS 2- Tìm hiểu openCV thư viện Point Cloud 3- Tìm hiểu thiết bị KINECT 4- Kết nối KINECT với máy tính 5- Thu thập ảnh chụp từ KINECT lưu dạng ảnh dạng đám mây 6- Tìm điểm tương đồng đám mây 3D 7- Ghép đám mây 3D để khơng gian hồn thiện 8- Chỉnh sửa thuật tốn, lọc nhiễu từ mơi trường để có không gian tốt 9- Viết báo cáo luận văn 10- Báo cáo đề tài tốt nghiệp GIỚI HẠN Đề tài hồn thành việc xây dựng khơng gian chưa xác định từ liệu ảnh thu camera KINECT Bên cạnh đó, khơng gian sau xây dựng bị nhiễu bị sai lệch nhiều so với khơng gian thực tế tác động yếu tố môi trường như: ánh sáng, độ chi tiết vật thể 1.5 BỐ CỤC Chương 1: Dẫn nhập Đưa vấn đề, lí chọn đề tài Đề tài có tính cấp thiết đời sống kết đạt sau hoàn thành với giới hạn đề tài Chương 2: Cơ sở lí thuyết Giới thiệu cách khái quát camera KINECT, thuật tốn có liên quan cơng cụ thư viện để xử lí ảnh 3D Đưa nguyên lí để xử lí điểm ảnh, đám mây thu thập từ KINECT Chương 3: Thiết kế Đưa sơ đồ khối chương trình, cách ghép nối đám mây lại để tạo thành không gian lớn Nêu cụ thể bước thực BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP 15 CHƯƠNG KẾT QUẢ - NHẬN XÉT - ĐÁNH GIÁ Hình 5.11 Ảnh 2D lối xếp có nhiều chi tiết Nhóm chụp lại 10 khung hình tương ứng với khơng gian định trước Tương ứng với không gian, Kinect di chuyển tịnh tuyến 10 lần lần di chuyển 30cm để chụp lại 10 khung hình tương ứng Sau đó, nhóm đưa bảng thống kê thể qua Bảng 5.4 Bảng 5.4 Bảng thống kê số lượng điểm đặc trưng Số lượng điểm đặc trưng lối Số lượng điểm đặc trưng lối đi tự nhiên xếp nhiều chi tiết 462 929 497 840 459 914 445 1052 459 848 652 718 441 777 542 837 529 960 10 492 831 Khung hình BỘ MƠN ĐIỆN TỬ CƠNG NGHIỆP 64 SỐ LƯỢNG ĐIỂM ĐẶC TRƯNG CHƯƠNG KẾT QUẢ - NHẬN XÉT - ĐÁNH GIÁ BIỂU ĐỒ THỂ HIỆN CHÊNH LỆCH SỐ LƯỢNG ĐIỂM ĐẶC TRƯNG 1200 1000 1052 929 840 914 718 652 800 600 960 848 462 497 459 445 459 777 441 837 831 542 529 492 400 200 10 KHUNG HÌNH Số lượng điểm đặc trưng lối tự nhiên Số lượng điểm đặc trưng lối xếp nhiều chi tiết Hình 5.12 Biểu đồ thống kê chênh lệch số lượng điểm đặc trưng khung hình chụp Đồng thời từ Bảng 5.4, nhóm biểu đồ thống kê Hình 5.12 để so sánh chênh lệch số lượng điểm đặc trưng cửa khung hình tương ứng Ta thấy Hình 5.12 số lượng điểm đặc trưng khung hình chụp khơng gian lối xếp có nhiều chi tiết nhiều hẳn số lượng điểm đặc trưng khung hình chụp khơng gian lối tự nhiên Điều cho ta biết đồ không gian 3D xây dựng cách chi tiết hơn, xác hồn thiện khơng gian có nhiều đặc điểm chi tiết Đối với vùng khơng gian có chi tiết khơng có chi tiết, đồ xây dựng tái khơng xác bị sai lệch với khơng gian thực tế nhiều so với khơng gian có nhiều chi tiết BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP 65 CHƯƠNG KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN Chương KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 6.1 KẾT LUẬN Đề tài hồn thành việc xây dựng thành cơng khơng gian chưa xác định thành đồ 3D, cụ thể khơng gian phịng học Bên cạnh đó, đề tài trình bày sở lí thuyết phương pháp để xây dựng thành công đồ 3D từ không gian chưa biết kết hợp sử dụng camera Kinect Từng khung ảnh 2D xử lí thuật tốn SIFT để xác định điểm đặc trưng cặp điểm tương đồng khung ảnh 2D Từ đó, tìm điểm đặc trưng cặp điểm tương đồng tương ứng đám mây 3D Sau ghép đám 3D dựa vào cặp điểm tương đồng đám mây lớn thể khung cảnh vùng không gian nhiều Cảm biến Kinect có giá thành rẻ nhiều so với cảm biến 3D loại, có khả quan sát tương tự mắt người, thuật toán sử dụng cảm biến Kinect để xác định vùng khơng gian di chuyển có ứng dụng rộng rãi lĩnh vực nhận dạng định vị khơng gian Cụ thể hơn, nhắc tới ứng dụng cho Robot tự hành giúp Robot vừa di chuyển vừa chụp hình mơ tả lại vùng khơng gian mà di chuyển đồ 3D dựng lên Như nói đầu đề tài, để Robot tự hành hoạt động tốt việc phức tạp, với đề tài góp phần vào việc xây dựng phần Robot Đề tài chạy thực nghiệm việc xây dưng lại không gian phịng học với điều kiện mơi trường tốt Nhóm di chuyển camera Kinect đoạn ngắn, chụp lại ảnh khung cảnh xử lí Để đảm bảo đạt kết tốt trình thực lặp lại nhiều lần kết so sánh với để rút kết luận Kết cho thấy đồ 3D sau hoàn thành chi tiết hình ảnh vùng khơng gian tương đối rõ ràng Nhưng bên cạnh cịn khuyết điểm cần cải tiến thuật toán để đồ sau xây dựng hồn thiện tốt BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP 66 CHƯƠNG KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 6.2 HƯỚNG PHÁT TRIỂN Đối với đề tài “Xây dựng đồ không gian chưa xác định sử dụng camera Kinect” hướng để phát triển thêm Bên cạnh việc mà đề tài thực cịn có khuyết điểm tồn Đầu tiên phải nhắc đến hệ thống phần cứng để chạy thực nghiệm đề tài, việc phải chạy thực nghiệm với máy tính có cấu hình phần cứng trung bình tốn nhiều thời gian phải đợi máy tính chạy thuật tốn xử lí tương đối phức tạp Nếu muốn dựng khơng gian lớn với thời gian hồn thành nhanh cần phải có hệ thống phần cứng đủ mạnh để thực thuật tốn xử lí cách nhanh chóng Bên cạnh yếu tố phần cứng vấn đề phần mềm, đề tài nhóm sử dụng thuật tốn SIFT để xác định điểm đặc trưng cặp điểm tương đồng ta tìm hiểu sử dụng thuật tốn khác có khả tương tự xử lí nhanh hơn, xác hiệu Cuối cùng, yếu tố môi trường ảnh chụp từ camera Kinect mà hình ảnh khơng gian 3D sau dựng lên bị nhiễu bị sai lệch so với thực tế Để có kết đồ 3D hoàn hảo, cần loại bỏ yếu tố gây nhiễu đồ 3D việc kết hợp thuật toán lọc nhiễu để xử lí khơng gian 3D trước cho kết cuối BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP 67 TÀI LIỆU THAM KHẢO TÀI LIỆU THAM KHẢO [1] Shinya OGAWA, Kajiro WATANABE, Kazuyuki KOBAYASHI, “2D Mapping of a Closed Area by a Range Sensor”, SICE, 2002 [2] Felix Endres, Jurgen Hess, J ă urgen Sturm, Daniel Cremers, and Wolfram Burgard, “3-D Mapping With an RGB-D Camera”, IEEE Transactions On Robotics, 2013 [3] Peter Beňo, František Duchoň, Michal Tölgyessy, Peter Hubinský, Martin Kajan, “3D map reconstruction with sensor Kinect”, 23rd International Conference on Robotics in Alpe-Adria-Danube Region, 2014 [4] Hesham Ibrahim, Mohamed Ahmed Omara, Khairul Salleh Mohamed Sahari, “Indoor Mapping using Kinect and ROS”, International Symposium on Agents, Multi-agent Systems and Robotics (ISAMSR), 2015 [5] Morgan Quigley, Brian Gerkey, William D Smart, Programming Robots with ROS: A Practical Introduction to the Robot Operating System, O'Reilly Media, 2015 [6] Samarth Brahmbhatt, “Practical OpenCV”, Apress, 2013 BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP 68 PHỤ LỤC PHỤ LỤC #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include using namespace cv; ////////////////////////////////////////////////////////////////////////////////// typedef pcl::PointXYZRGB PointType; typedef pcl::PointCloud PointCloud; PointCloud::Ptr inputCloud (new PointCloud); PointCloud::Ptr outputCloud (new PointCloud); PointCloud::Ptr firstCloud (new PointCloud); PointCloud::Ptr secondCloud (new PointCloud); BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP 69 PHỤ LỤC Mat inputImage; Mat firstImage; Mat secondImage; bool initial = true; pcl::visualization::PCLVisualizer viewer; ros::Publisher cloud_pub; ros::Publisher image_pub; ////////////////////////////////////////////////////////////////////////////////// void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& inputROSCloud) { pcl::fromROSMsg (*inputROSCloud, *inputCloud); viewer.removePointCloud ("outputCloud"); viewer.addPointCloud (outputCloud, "outputCloud"); viewer.spinOnce (); return; } ////////////////////////////////////////////////////////////////////////////////// cv_bridge::CvImagePtr cv_ptr; void image_cb (const sensor_msgs::ImageConstPtr& msg) { cv_ptr = cv_bridge::toCvCopy (msg, sensor_msgs::image_encodings::BGR8); inputImage = cv_ptr->image; imshow ("ImageVisualization", inputImage); return; } ////////////////////////////////////////////////////////////////////////////////// void detectMatchingImagePoints ( Mat FirstImage, Mat SecondImage, vector &FirstMatchingImagePoints, vector &SecondMatchingImagePoints) { std::cout