PCL Iterative Closest Point 튜토리얼 분석

Point Cloud Library에서 제공하는 Iterative Closest Point (ICP)의 튜토리얼 코드를 실행해보고 분석하겠습니다.

튜토리얼 코드는 아래의 페이지에 있습니다.

 

pointclouds 공식 홈페이지 tutorials - iterative_closest_point

Documentation - Point Cloud Library (PCL)

ICP 알고리즘은 서로 다른 두 matrix A와 B 간의 거리를 최소화하는 알고리즘입니다.

transposed A와 B를 곱하고 그것을 Singular Value Decomposition(SVD) 해서 U, S, Vt를 구하고 transposed Vt와 transposed U를 곱한 행렬이 회전 행렬(Transformation matrix)이 됩니다.

ICP 설명

블로그에서는 식을 쓰기가 어려워서 위의 그림으로 대체하겠습니다.

더 자세한 설명은 논문이나 다른 블로그를 참고해주세요.

 

간단하게 ICP를 설명했으니 튜토리얼 코드에 대해서 분석해보겠습니다.

 

먼저 무작위로 6개의 데이터를 생성하고 matrix A에 넣습니다.

그리고 그 중 5개에 더하기와 곱하기를 해서 변형을 해주고 matrix B에 넣습니다.

cloud_out->points[i].x = cloud_in->points[i].x + 0.3f;
cloud_out->points[i].y = cloud_in->points[i].y + 0.5f;
cloud_out->points[i].z = cloud_in->points[i].z * 0.9f;

 

cloud_in을 matrix A로, cloud_out을 matrix B로 세팅한 후 ICP 알고리즘을 실행합니다.

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
icp.setMaximumIterations (1);
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);

아래는 실행 결과입니다.

왼쪽이 ICP tutorial을 돌린 결과입니다.

결과

matrix A와 matrix B, transformed matrix A와 매치된 점들 간의 거리의 합 그리고 transformation matrix를 출력합니다.

위 사진의 오른쪽은 제가 파이썬으로 transposed A * B에 SVD를 한 결과입니다.

V* Ut와 Transformation matrix가 같은 것을 확인할 수 있습니다.

matrix A, matrix B와 transformed A를 matplotlib로 그려봤습니다.

결과

애초에 z점에 *0.9를 했기 때문에 완전히 같아질 수는 없고, 비슷한 위치로 이동한 것을 확인할 수 있습니다.

 

제가 예전에 공부한 것을 두서없이 적은 것 같습니다.

더 자세한 이론적인 배경을 알고싶다면 논문을 꼭 읽으셔야 합니다.

그리고 EigenValue Decomposition -> SingularValue Decomposition으로 이어지는 이론과 각각의 성질들을 알고있어야합니다.
저는 선형대수학 이론이 부족해서 많이 어려웠었습니다..ㅠㅠ

---

잘못된 내용이 있다면 언제든지 댓글이나 메일로 알려주시면 감사하겠습니다.

이 포스팅이 도움이 되었다면 공감 부탁드립니다.

궁금한 점은 언제든지 댓글 남겨주시면 답변해드리겠습니다:D