Xem mẫu

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