Đánh giá giải thuật Iterative Closest Point trong xây dựng mô hình 3D

Bài viết thực hiện đánh giá và so sánh các giải thuật ICP - Iterative Closest Point được sử dụng để xây dựng mô hình 3D từ tập dữ liệu Point Cloud Data - PCD thu được từ camera LiDAR sau những lần quét liên tiếp nhau. Ba giải thuật gồm ICP gốc, ICP k-d tree, ICP ak-d tree được đánh giá, so sánh dựa trên các thông số là sai số và tổng thời gian chạy giải thuật. Kết quả cho thấy giải thuật ICP k-d tree và ak-d tree thực hiện cho tổng thời gian thực hiện ít hơn 10 lần, với độ chính xác cao hơn 20 lần so với giải thuật ICP gốc. Mời các bạn cùng tham khảo!

Đánh giá Giải thuật Iterative Closest Point Xây dựng mơ hình 3D Đặng Ngọc Hạnh[1], Nguyễn Trung Hiếu[2] Khoa Điện – Điện tử, Trường Đại học Bách khoa – ĐHQG-HCM Email: [1] hanhdn@hcmut.edu.vn, [2] trhieu1597@gmail.com Abstract— Trong báo này, thực đánh giá so sánh giải thuật ICP - Iterative Closest Point sử dụng để xây dựng mơ hình 3D từ tập liệu Point Cloud Data - PCD thu từ camera LiDAR sau lần quét liên tiếp Ba giải thuật gồm ICP gốc, ICP k-d tree, ICP ak-d tree đánh giá, so sánh dựa thông số sai số tổng thời gian chạy giải thuật Kết cho thấy giải thuật ICP k-d tree ak-d tree thực cho tổng thời gian thực 10 lần, với độ xác cao 20 lần so với giải thuật ICP gốc Kết cụ thể phụ thuộc vào tổng số điểm liệu, nhiễu tác động vào tập liệu Keywords- ICP, Iterative Closest Point, LiDAR, Point Cloud Data, Matching Scanner, k-d tree, ak-d tree… I GIỚI THIỆU Việc tái tạo xây dựng lại mơ hình 2D 3D lĩnh vực nghiên cứu phát triển nhiều với ứng dụng tiềm việc dự đoán khắc phục hậu thiên tai, thám hiểm, trắc địa, … [1-3] Đặc biệt hệ thống robot tự động, camera đóng vai trò cảm biến thị giác giúp robot nhận biết mơi trường xung quanh thơng qua việc xây dựng lại mơ hình đồ (bản đồ môi trường xung quanh robot hoạt động) Xây dựng mơ hình đồ giúp giải tốn định vị robot mơi trường mà hoạt động, giúp robot tránh vật cản [4] Hình Hình minh họa sử dụng PCD để dựng lại mơ hình 3D cùa phịng [6] Hiện giới có nhiều nghiên cứu viết giải thuật ICP ứng dụng [7-13] Năm 1992, tác giả Paul J Besl and Neil D McKay [7] đưa nghiên cứu kinh điển giải thuật ICP, với định nghĩa cụ thể mô hình giải thuật lặp ICP với bước giải thuật Giải thuật chứng minh tính hội tụ hạn chế tốc độ, cụ thể bước tìm điểm tương đồng Để cải thiện tốc độ tính tốn, Besl McKay đề xuất phương pháp sử dụng tìm kiếm đa chiều (kd tree) [8] cải thiện tốc độ giải thuật bước tiềm kiếm điểm tương đồng Tuy nhiên, số điểm tập liệu tăng lên lớn việc xây dựng tìm kiếm đa chiều trở nên khó khăn địi hỏi tốn nhiều nhớ Để tăng hiệu cho ICP, phương pháp xấp xỉ approximate k-d tree (ak-d tree) cho tốc độ tính tốn nhanh k-d tree sai số số lần lặp giảm khơng đáng kể so với k-d tree [9] Giải thuật cached k-d tree cải tiến từ k-d tree cho thời gian chạy giải thuật nhanh khoảng 50% so với giải thuật k-d tree [10] Ngoài ra, việc tái tạo lại mơ 3D cịn có phương pháp sử dụng để tìm điểm tương đồng như: phương pháp khai thác cấu trúc hình học vật thể Go-ICP [11], phương pháp giảm số mẫu phát điểm tương đồng sở đặc trưng hình học [12] LiDAR- Light Detection and Ranging công nghệ kết hợp công nghệ laser, GPS IMU Dữ liệu mà Camera LiDAR thu nhận tập hợp điểm có tọa độ theo ba trục hệ hệ tọa độ xác định gọi PCD [5] Ưu điểm trội xây dựng mơ hình bề mặt có độ xác cao, mật độ liệu điểm lớn, chí lớn đảm bảo tính chi tiết địa hình thực tế (Hình 1) Q trình tái tạo mơ hình khơng gian xung quanh robot theo lý thuyết việc ghép liệu từ lần quét liên tiếp robot dịch chuyển Trong thực tế, nhiễu tác động làm cho liệu từ hai lần quét liên tiếp có sai số gây khó khăn cho việc ghép nối tái tạo mơ hình Để giải vấn đề trên, giải thuật ICP đề xuất, cho phép tìm kiếm cặp điểm tương đồng với hai tập liệu từ loại bỏ điểm có sai số lớn nhiễu tác động 142 Ứng dụng giải thuật ICP dùng liệu thu từ camera LiDAR nghiên cứu ứng dụng cho robot tự hành, UAV, … [4,13,14] Giải thuật ICP kết hợp với SLAM (Simultaneous Localization and Mapping) nghiên cứu xây dựng đồ mơi trường xung quanh [4] Trong đó, 𝑑 sai số phép tìm kiếm Từ định nghĩa (2), điểm gọi tương đồng hai tập liệu điểm có khoảng cách chúng ngắn Việc tìm điểm tương đồng chinh giải tốn tìm khoảng cách ngắn hai tập điểm Ở giải thuật ICP gốc, tốn tìm kiếm thực dựa việc so sánh khoảng cách hình học Eulcide điểm thuộc tập liệu P với tất điểm thuộc tập Q, từ ta chọn cặp điểm cho khoảng cách ngắn Trong báo này, giải thuật ICP gồm ICP gốc hai giải thuật cải tiến k-d tree ak-d tree nghiên cứu lại Sau đó, tác giả thực mô để đánh giá, so sánh giải thuật dùng thông số cụ thể gồm: thời gian chạy giải thuật sai số thuật toán Kết việc đánh giá, so sánh làm tiền đề cho việc lựa chọn giải thuật ICP phù hợp với yêu cầu ứng dụng cụ thể Bài toán 2: Giả sử biết điểm tương đồng, ma trận 𝑅 vector 𝑡 tính tốn cho sai số 𝐸(𝑅, 𝑡) cực tiểu Đây toán tối ưu cho hàm (1) dựa thuật toán SVD (singular value decomposition) Ký hiệu: Ở phần tiếp theo, báo trình bày theo cấu trúc sau Giải thuật ICP gốc hai cải tiến giải thuật ICP trình bày phần II Ở phần III, báo trình bày kết thực giải thuật tập sở liệu mẫu với thông số đánh giá cụ thể nhận xét Cuối cùng, kết luận báo phần IV (𝑅, 𝑡, 𝑒) = Τ(𝑄, 𝑌) Với 𝑒 sai số thuật toán sai số cần tìm Lưu đồ giải thuật ICP qua vòng lặp thể Hình Trong giải thuật có hai sai số dùng để đánh giá kết giải thuật 𝑑 𝑒, tương ứng sai số việc tìm điểm tương đồng sai số cần tìm thuật toán II GIẢI THUẬT ICP Trong phần này, chúng tơi trình bày giải thuật ICP gốc hai giải thuật ICP cải tiến dùng k-d tree ak-d tree 1) Giải thuật ICP gốc Gọi 𝑃 = {𝑝 }, 𝑖 = 1,2, … , 𝑀 𝑄 = 𝑞 , 𝑗 = 1,2, … , 𝑁 hai tập liệu liên tiếp thu từ camera LiDAR Các điểm 𝑝 𝑞 điểm tọa độ hệ dercartes Giả sử, ta gọi 𝑃 𝑄 tập mẫu tập liệu thu Mục tiêu giải thuật ICP tìm phép biến hình để đưa 𝑄 trùng 𝑃 có thể, tức tìm ma trận xoay 𝑅 × vector tịnh tiến 𝑡 ∈ ℝ , cho hàm sai số (1) sau nhỏ nhất: 𝐸(𝑅, 𝑡) = ∑ 𝑅𝑞 + 𝑡 − 𝑝 ∗ (1) Trong đó, 𝑝 ∗ ∈ 𝑃 điểm tương đồng với 𝑞 tính cho tối ưu hàm sai số theo công thức (2): 𝑝 ∗ = 𝑎𝑟𝑔𝑚𝑖𝑛 𝑅𝑞 + 𝑡 − 𝑝 (2) , ,…, Cụ thể giải toán ICP giải hai tốn sau: Bài tốn 1: Đi tìm điểm tương đồng với tập liệu mẫu 𝑃 từ tập 𝑋 tập có sau thực phép biến hình lên 𝑄 Ký hiệu: 𝑋 = Γ(𝑄, 𝑅, 𝑡) (3) Trong đó, 𝑋 = 𝑥 thỏa 𝑥 = 𝑅𝑞 + 𝑡 với ma trận xoay 𝑅 cố định 𝑡 vector tịnh tiến Gọi kết phép tìm điểm tương đồng 𝑌 Tập 𝑌 tập 𝑋 xếp lại theo thứ tự hàng cho vị trí thứ 𝑗 𝑌 tương đồng với hàng vị trí thứ 𝑗 tập 𝑃 Ký hiệu: (𝑌, 𝑑) = 𝛬(𝑋, 𝑃) (5) (4) Hình Lưu đồ giải thuật ICP 143 - Giả sử, bước thứ 𝑘 giải thuật, tập liệu sau thực phép biến hình bước trước 𝑄 tập mẫu 𝑃 Tập điểm tương đồng tìm sau giải tốn tìm điểm tương đồng 𝑌 sai số việc tìm điểm tương đồng 𝑑 định nghĩa sau: 𝑑 = ∑ - 𝑦 , −𝑞 (6) , Tiếp theo, bước tính tốn phép biến hình cho ta ma trận 𝑅 𝑡 Sau thực phép biến hình ta có tập điểm 𝑄 Sai số sau lần lặp bước thứ 𝑘 sai số hai tập 𝑌 𝑄 𝑒 = ∑ 𝑦 , −𝑞 , Hình (a) Không gian mặt phẳng thực chia chiều (7) Thuật toán kết thúc với điều kiện hiệu số 𝑒 𝑒 nhỏ ngưỡng yêu cầu Ngược lại, thuật toán quay lại thực lại từ bước tính tốn điểm tương đồng 2) Giải thuật ICP k-d tree Giải thuật ICP k-d tree xây dựng dựa giải thuật ICP gốc Điểm cải tiến bước tìm điểm tương đồng, thay phải duyệt hết tất điểm để tìm khoảng cách ngắn nhất, giải thuật tìm kiếm tìm kiếm đa chiều (k-d tree searching) sử dụng Giải thuật tìm kiếm k-d tree gồm hai phần: xây dựng tìm kiếm đa chiều tìm kiếm Hình (b) Biểu diễn điểm tìm kiếm đa chiều a) Xây dựng đa chiều: b) Thực tìm kiếm cây: Để đơn giản, tác giả trình bày tìm kiếm hai chiều thể Hình (a) Ý tưởng chia mặt phẳng chứa điểm thành vùng ngăn cách đường thẳng x, y liên tiếp - Bước 1, ta chia mặt phẳng thành hai phần đường thẳng 𝑥 giá trị trung bình tồn điểm lấy theo trục x - Bước 2, vùng bên trái, ta chia tiếp thành hai phần đường 𝑦 giá trị trung bình tồn điểm nằm vùng lấy theo trục y Tương tự, vùng bên phải, ta chia thành hai phần đường thẳng 𝑦 - Bước 3, vùng (y), ta lại chia tiếp thành hai phần đường thẳng x giá trị trung bình tồn điểm vùng - Bước 4, vùng (x) ta lại chia theo y, - Việc chia dừng lại vùng mặt phẳng chứa điểm không chưa điểm Việc xây dựng cho hai chiều cân Gọi 𝑝 , 𝑞 điểm thuộc tập mẫu P tập liệu Q tương đồng với Ta tìm kiếm điểm 𝑞 tập liệu qua vùng không gian mà làm cho khoảng cách từ 𝑞 đến 𝑝 ngày nhỏ bỏ qua tìm kiếm vùng không gian làm tăng khoảng cách từ 𝑝 đến 𝑝 Việc tìm kiếm trả điểm 𝑞 thỏa công thức: ‖𝑝 − 𝑞 ‖ ≤ ‖𝑝 − 𝑞‖ ∀𝑞 ∈ 𝑄 3) Giải thuật ICP ak-d tree Giải thuật ICP xấp xỉ nhị phân, ak-d tree xây dựng sở giải thuật ICP k-d tree tìm kiếm xấp xỉ tìm kiếm đa chiều Ở giải thuật tìm kiếm k-d tree, việc tìm kiếm trả điểm 𝑞 thỏa cơng thức (8) Trong khi, giải thuật ak-d tree cho kết điểm 𝑞 thỏa công thức: ‖𝑝 − 𝑞 ‖ ≤ (1 + 𝜀)‖𝑝 − 𝑞‖ ∀𝑞 ∈ 𝑄, > 𝜀 > (9) Trong đó, ε số chọn trước Kết biểu diễn lên tìm kiếm Hình (b) theo quy ước: - Khi chia theo trục x, điểm có giá trị x nhỏ nằm bên tay trái - (8) Từ cơng thức, ta thấy điểm cần tìm 𝑞 khơng thiết phải xác 𝑝 mà lệch khỏi 𝑝 hình cầu có bán kính ε Việc chọn ε quan trọng, ε lớn làm giảm nhanh việc tìm kiếm lại làm tăng sai số Nếu ε nhỏ việc tìm kiếm lâu sai số nhỏ Khi chia theo trục y, điểm có giá trị y nhỏ nằm bên tay trái 144 III MÔ PHỎNG VÀ ĐÁNH GIÁ Bảng Kết chạy giải thuật theo thông số bảng Các giải thuật ICP trình bày phần II, thực mơ dùng Matlab 2017b Việc đánh giá thông số thực chạy giải thuật laptop DELL có cấu hình processor Intel(R) Core (TM) i5-4210CPU 1.7GHz (4 CPUs), RAM 6GB NVIDIA GeForce GT 750M Tập liệu Dữ liệu M=4893 Dữ liệu tập điểm PCD mẫu P dùng để mô tập liệu The Stanford 3D Scanning Repository từ sở liệu LiDAR chuẩn trường đại học Standford Dữ liệu M=6539 1) Mơ hình mơ giải thuật Dữ liệu M=7517 Giải thuật ICP đánh giá dựa sai số ghép mơ hình tổng thời gian chạy giải thuật Đánh giá thực cố định số điểm liệu mức sai số ngưỡng Dữ liệu M=21158 Tập liệu giả lập tạo tập mẫu có độ chồng lấp 𝜇 (0 < μ < phần trùng hai tập) qua bước: - Bước 1, từ liệu ban đầu có số điểm Α, bỏ Aμ điểm vị trí đầu tập mẫu P; bỏ Aμ điểm vị trí cuối ta tập liệu Q - Bước 2, xoay tập Q góc α, β, γ theo ba trục x, y, z tịnh tiến a đơn vị - Bước 3, nhiễu thêm vào nhiễu có phân bố chuẩn với phương sai σ Giải thuật thực ICP gốc ICP k-d tree ICP ak-d tree ICP gốc ICP k-d tree ICP ak-d tree ICP gốc ICP k-d tree ICP ak-d tree ICP gốc ICP k-d tree ICP ak-d tree Sai số e 3,15040583.10 6,47913794.10 6,47915348.10 3,05225636.10 1,38933847.10 1,38936484.10 1, 83172742.10 2.71965519.10 2,71967832.10 2,54891638.10 5,17465378.10 5,17465651.10 Cụ thể tất thơng số chạy giải thuật trình bày Bảng sau Bảng Thông số dùng mô giải thuật ICP Độ chồng lấp 𝜇 𝛼, 𝛽, 𝛾 a 𝜎 Ngưỡng sai số 𝜏 Số lần lặp tối đa Hệ số 𝜀 92% 𝜋 𝜋 𝜋 , , 18 10 0,001 0,001 10 50 5% Hình (a) Dữ liệu trước chạy giải thuật 2) Kết chạy mô đánh giá Thực chạy mô giải thuật ICP gốc hai giải thuật ICP k-d tree, ak-d tree tập liệu với số điểm liệu M hình dạng PCD khác Kết thực mô gồm sai số tổng thời gian chạy giải thuật thực chạy mơ hình mơ 10 lần lấy kết tốt nhất, trình bày Bảng Kết thu cho thấy giải thuật ICP k-d tree ak-d tree cho tốc độ tính tốn nhanh từ 14 đến 23 lần Sai số giảm so với ICP gốc từ 22 đến 67 lần Độ xác thời gian cụ thể phụ thuộc vào số điểm liệu nhiễu thêm vào Kết thể không gian 3D thực mô tập liệu 3, thể Hình (a-c) sau Kết cho thấy sau chạy giải thuật ICP, hai tập P Q đưa gần trùng với kết ICP k-d tree tốt so với ICP gốc Hình (b) Dữ liệu chạy giải thuật ICP gốc 145 Thời gian t (s) 750,8 38,4 34,3 870,4 199,2 194,8 3050.8 136.3 125.5 8261,4 607,8 591,6 Kết nghiên cứu tảng cho việc xây dựng mơ hình đồ môi trường xung quanh dùng ứng dụng trắc địa, robot tự động, … Các tác giả định hướng phát triển tiếp nghiên cứu ứng dụng robot tự hành kết hợp ICP SLAM TÀI LIỆU THAM KHẢO Hình (c) Dữ liệu chạy giải thuật ICP k-d tree Để so sánh tốc độ hội tụ ba giải thuật, giải thuật thực mô dùng tập liệu 3, cố định số vòng lặp 30 0.00035 [1] Ryota Tsubaki and Ichiro Fujita, "Unstructured grid generation using LiDAR data for urban flood inundation modelling," Hydrol Process 24, pp 1404-1420, 2010 [2] Hermann M Fritz, David A Phillips, Akio Okayasu, etc, "The 2011 Japan tsunami current velocity measurements from survivor videos at Kesennuma Bay using LiDAR," GEOPHYSICAL RESEARCH LETTERS, vol 39, 2012 [3] F Rottensteiner, Ch Briese, " A new method for building extraction in urban areas from high-resolution LIDAR data," Commission III, WG III/3 [4] Manuel González Ocando, Novel Certad, Said Alvarado and Ángel Terrone, "Autonomous 2D SLAM and 3D Mapping of an Environment Using a Single 2D LIDAR and ROS," IEEE, 2017 [5] Behnam Behroozpour, Phillip A M Sandborn, Ming C Wu, Bernhard E Boser , "Lidar System Architectures and Circuits," IEEE Communications Magazine, vol 55, no 10, pp 135 - 142, 2017 [6] Radu Bogdan Rusu, Steve Cousins , "3D is here: Point Cloud Library (PCL)," 2011 IEEE International Conference on Robotics and Automation, 2011 [7] Paul J Besi and Neil D McKay, "A Method for Registraion of 3D Shapes," IEEE TRANSACTIONS ON PATTERN AND MACHINE INTELLIGENCE, vol 14, pp 239-256, 1992 [8] J L Bentley, "Multidimensional Binaru Search Trees Used for Associative Searching," ACM Student Award, 1975 [9] Michael Greenspan, Mike Yurick, "Approximate K-D Tree Search for Efficient ICP," Proceedings of the Fourth International Conference on 3-D Digital Imaging and Modeling, 2003 IEEE 0.0003 0.00025 0.0002 0.00015 0.0001 0.00005 0 10 ICP gốc 15 20 ICP k-d tree 25 30 35 ICP ak-d tree [10] Andreas Nuchter, Kai Lingemann, and Joachim Hertzberg, "Cached k-d tree search for ICP algorithms," Sixth International Conference on 3-D Digital Imaging and Modeling, 2007 IEEE Hình Đồ thị biểu diễn sai số vào vòng lặp liệu [11] Jiaolong Yang, Hongdong Li, Yunde Jia, "Go-ICP: Solving 3D Registration Efficiently and Globally Optimally" Kết sai số vòng lặp thể Hình cho thấy giải thuật ICP k-d tree ak-d tree cho sai số nhỏ 2.7.10 so với ICP gốc cho sai số khoảng 1, 8.10 hội tụ Do việc chọn hệ số ε = 5% hoàn toàn phù hợp [12] Ying He, Bin Liang, Jun Yang, Shunzhi Li and Jin He, "An Iterative Closest Points Algorithm for Registration of 3D Laser Scanner Point Clouds with Geometric Features," Sensors 2017 IV [13] Héber Sobreira, Carlos M Costa, Ivo Sousa, Luis Rocha, José Lima, P C M A Farias,Paulo Costa, "Map-Matching Algorithms for Robot Self-Localization," Journal of Intelligent & Robotic Systems, 2018 KẾT LUẬN Bài báo thực so sánh ba giải thuật ICP gốc, ICP k-d tree ICP a-k tree sai số tốc độ thực giải thuật Kết đánh giá so sánh cho thấy giải thuật ICP k-d tree akd tree có chất lượng tốt so với ICP gốc [14] Gu Tianyuan, Zhang Ning, "Application of Iterative Closest Point Algorithm in Automatic Flight of Speedy UAV," Proceedings of 2014 IEEE Chinese Guidance, Navigation and Control Conference, pp 1456 - 1459, 2014 IEEE 146 ... qua vịng lặp thể Hình Trong giải thuật có hai sai số dùng để đánh giá kết giải thuật