Computer rotation matrix use SVD (2): Finding optimal rotation and translation between corresponding

最新推荐文章于 2022-11-26 15:09:51 发布

ccxcau

最新推荐文章于 2022-11-26 15:09:51 发布

阅读量1.1k

点赞数

分类专栏： Kinect mathematics info Robotics 文章标签： matrix dataset transformation translation distance matlab

本文链接：https://blog.csdn.net/ccxcau/article/details/7346669

版权

Kinect 同时被 3 个专栏收录

15 篇文章 0 订阅

订阅专栏

mathematics info

9 篇文章 0 订阅

订阅专栏

Robotics

2 篇文章 0 订阅

订阅专栏

Finding optimal rotation and translation between corresponding 3D points

Finding the optimal/best rotation and translation between two sets of corresponding 3D point data, so that they are aligned/registered, is a common problem I come across. An illustration of the problem is shown below for the simplest case of 3 corresponding points (the minimum required points to solve).

The corresponding points have the same colour, R is the rotation and t is the translation. We want to find the best rotation and translation that will align the points in dataset A to dataset B. Here, ‘optimal’ or ‘best’ is in terms of least square errors. This transformation is sometimes called the Euclidean or Rigid transform, because it preserves the shape and size. This is in contrast to an affine transform, which includes scaling and shearing.

This problem arises especially in tasks like 3D point cloud data registration, where the data is obtained from hardware like a 3D laser scanner or the popular Kinect device. The solution I’ll be presenting is from the paper‘A Method for Registration of 3-D Shapes’, by Besl and McKay, 1992.

Solution overview

Finding the optimal rigid transformation matrix can be broken down into the following steps:

Find the centroids of both dataset
Bring both dataset to the origin then find the optimal rotation, (matrix R)
Combine R and centroids into a single 4×4 matrix transformation

Finding the centroids

This bit is easy, the centroids are just the average point and can be calculated as follows:

$P = \left[\begin{array}{c} x\\ y\\ z \end{array}\right]$

$centroid_A = \frac{1}{N} \displaystyle\sum\limits_{i=1}^N P_A^i$

$centroid_B = \frac{1}{N} \displaystyle\sum\limits_{i=1}^N P_B^i$

Here, $P_A$ and $P_B$ are points in dataset A and B respectively. We will use these values in the next step.

Finding the optimal rotation

There are a few ways of finding optimal rotations between points. The easiest way I found is using Singular Value Decomposition (SVD), because it’s a function that is widely available in many programming languages (Matlab, Octave, C using LAPACK, C++ using OpenCV …). SVD is like this powerful magical wand (Harry Potter ain’t got nothing on the SVD) in linear algebra for solving all sorts of numerical problems, but tragically wasn’t taught when I was at uni. I won’t go into details on how it works but rather how to use it. You only need to know that the SVD will decompose/factorise a matrix (call it E), into 3 other matrices, such that:

$[U,S,V] = SVD(E)$

$E = USV^T$

If E is a square matrix then U, S and V are the same size as well. We’re only interested in square matrices for this problem so I won’t go into detail about rectangular ones.

To find the optimal rotation we first re-centre both dataset so that both centroids are at the origin, like shown below.

This removes the translation component, leaving on the rotation to deal with. The next step involves accumulating a matrix, called H, and using SVD to find the rotation as follows:

$H = \displaystyle\sum\limits_{i=1}^N (P_A^i - centroid_A)(P_B^i - centroid_B)^T$

$[U,S,V] = SVD(H)$

$R = VU^T$

At this point you may be thinking, “what the, that easy?!”, and indeed you would be right. One thing to be careful is that you calculate H correctly. It should end up being a 3×3 matrix, not a 1×1 matrix. Pay close attention to the transpose symbol. It’s doing a multiplication between 2 matrices where the dimensions effectively are, 3×1 and 1×3, respectively. The ordering of the multiplication is also important, doing it the other way will find a rotation from B to A instead.

Combing R and centroids

So we’ve now got matrix R and t. The last and final step is to combine them together to get a single convenient matrix transformation, call it T. This will require TR to be a 4×4 matrix, which means R (3×3) and t (3×1) have to be converted to a 4×4 matrix by padding zeros and ones appropriately. The complete step is outlined below:

$C_{A} = \left[ \begin{array}{ccc|c} 1 & 0 & 0 & \\ 0 & 1 & 0 & -centroid_A\\ 0 & 0 & 1 & \\ 0 & 0 & 0 & 1 \end{array} \right]$

$R_{new} = \left[ \begin{array}{ccc|c} & & & 0 \\ & R & & 0\\ & & & 0 \\ 0 & 0 & 0 & 1 \end{array} \right]$

$C_{B} = \left[ \begin{array}{ccc|c} 1 & 0 & 0 & \\ 0 & 1 & 0 & centroid_B\\ 0 & 0 & 1 & \\ 0 & 0 & 0 & 1 \end{array} \right]$

$T = C_B R_{new} C_A$

That completes the solution.

Note on usage

The solution presented can be used on any size dataset as long as there are at least 3 points. When there are more than 3 points a least square solution is obtained, such that the following error is minimised:

$r = \displaystyle\sum\limits_{i=1}^N \left||TP_A^i -P_B^i \right||^2$

Here, the operator || || is the Euclidean distance between two vectors, a scalar value. r is just the square distance error between the points in the two dataset. I’ve abused the notion a bit (naughty me). P in this case is a 4×1 column vector (instead of the 3×1 matrix defined earlier), with the last value being 1, a homogeneous point.

Code

This script has been tested in Octave. It should work in Matlab but it has not been tested.

Download rigid_transform_3D.m

Read the comments in the file for usage.

ccxcau

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
1
评论
Computer rotation matrix use SVD (2): Finding optimal rotation and translation between corresponding

Finding optimal rotation and translation between corresponding 3D pointsFinding the optimal/best rotation and translation between two sets of corresponding 3D point data, so that they are aligne
复制链接

扫一扫

专栏目录