Similarity Transformation estimation

In this exercise the main topic is the estimation of an transformation between two point clouds(left and right).This page is available for practice as an interactive jupyter notebook.

${\Large r = sRl + t }$

$l$: a 3D point from the left point cloud, $l\in\mathbb{R}^3$
$r$: a 3D point from the right point cloud, $r\in\mathbb{R}^3$
$R$: a $3\times3$ rotation matrix
$t$: a 3D vector representing the translation, $t\in\mathbb{R}^3$
$s$: a scale, $s\in\mathbb{R}$

Transformation estimation

use the following algorithm (recipe) to estimate similarity transform from point correspondences. Given are two points list wtih same number of points $l_i,r_i$ with $i\in1,2,…,n$. Our goal ist to estimate $R,s$ and $t$ and there fore we define an optimization problem. We need to find parameters which minimize the distance between the points.

${\Large\sum_{i}{||(sRl_i + t) -r_i||}^{2} \stackrel{!}{=} min }$

Steps:

Reduce both point lists by their center of mass
Compute the rotation
Compute the scale
Compute the translation

Reduction my the center of mass

Compute centers of mass as follows (Try not to use loops):

${\Large l_{com}=\frac{1}{n}\sum_{i}l_i}$ ${\Large r_{com}=\frac{1}{n}\sum_{i}r_i}$

Then reduce all points by the calculatied center of mass.

${\Large l_i' = l_i - l_{com}}$ ${\Large l_i' = r_i - r_{com}}$

# imports
from plyfile import PlyData
import numpy as np
from tools import rotmat_from_quat #You can use the predefined function as follows:  R = rotmat_from_quat(q)
                                   # It will be needed in the application part.

def com(points): 
    # center  of mass of the points
    # YOUR CODE HERE
    raise NotImplementedError()

tp = np.array([( 1, 0, 0), 
               (-1, 0, 0),
               ( 0, 1, 0), 
               ( 0,-1, 0),
               ( 0, 0, 1), 
               ( 0, 0, -1)
               ])
twos = np.array((2,2,2))
threes = np.array((3,-3,3))
assert np.array_equal(com(tp + twos), twos)
assert np.array_equal(com(tp + threes), threes)

Compute Rotation

Estimate the rotation quaternion $q$ as follows:

Calculate the sum functions $S_{xx}=\sum_{i}x_{l,i}’x_{r,i}’$, $S_{xy}=\sum_{i}x_{l,i}’y_{r,i}’$ … Generalized: $S_{ab}=\sum_{i}a_{l,i}’b_{r,i}’$. $a$ is left dimension and $b$ right dimension.
Compose matrix $S$ from the above $S_{ab}$ with $a,b\in\{x,y,z\}$.

$S = \begin{bmatrix} S_{xx}+S_{yy}+S_{zz}& S_{yz}-S_{zy} & S_{zx}-S_{xz} & S_{xy}-S_{yx} \\ \cdotp & S_{xx}-S_{yy}-S_{zz} & S_{xy}+S_{yx} & S_{zx}+S_{xz} \\ \cdotp & \cdotp & -S_{xx}+S_{yy}-S_{zz} & S_{yz}+S_{zy} \\ \cdotp & \cdotp & \cdotp & -S_{xx}-S_{yy}+S_{zz} \end{bmatrix}$

Eigenvector of the largest eigenvalue of the S is the searched rotation quternion $q$.
The $3\times3$ rotation Matrix R is computed from q.

# Implement the sum function.
def sum_S(left_list, right_list):
    # YOUR CODE HERE
    raise NotImplementedError()

YOUR ANSWER HERE

assert sum_S(np.array([1,2,1]),np.array([1,4,3])) == 12
assert sum_S(np.array([1,2,1]),np.array([1,1,3])) == 6

# Implement the S matrix
def compose_S(left, right):
    # YOUR CODE HERE
    raise NotImplementedError()

def q_from_S(S):
    # Take a look at numpy.linalg.eig() function
    # YOUR CODE HERE
    raise NotImplementedError()

YOUR ANSWER HERE

tp = np.array([[ 1, 0, 0], 
               [-1, 0, 0],
               [ 0, 1, 0], 
               [ 0,-1, 0],
               [ 0, 0, 1], 
               [ 0, 0, -1]
               ])
rot_z_90 = np.array([[ 0,   1.0, 0  ],
                     [-1.0, 0,   0  ],
                     [ 0,   0,   1.0]
                    ])
rot_x_90 = np.array([[ 1,  0,  0],
                                [ 0,  0,  1],
                                [ 0, -1,  0]])
rot_y_90 = np.array([[ 0,  0,  1],
                                [ 0,  1,  0],
                                [ -1,  0, 0]])
tp_rot = np.dot(rot_z_90, np.transpose(tp)).transpose()
q = q_from_S(compose_S(tp, tp_rot))
assert np.allclose(q, np.array([ 0.70710678,  0.,          0.,         -0.70710678]))
tp_rot = np.dot(rot_y_90, np.transpose(tp)).transpose()
q = q_from_S(compose_S(tp, tp_rot))
assert np.allclose(q, np.array([ 0.70710678,  0.,          0.70710678,  0.,     ]))
tp_rot = np.dot(rot_x_90, np.transpose(tp)).transpose()
q = q_from_S(compose_S(tp, tp_rot))
assert  np.allclose(q, np.array([ 0.70710678, -0.70710678,  0.,          0.,        ]))

Compute scale

Compute the scale as follows:

${\Large s = \sqrt{\frac{\sum_{i}||r_i'||^{2}}{\sum_{i}||l_i'||^{2}}}}$

def calc_scale(left, right):
    # YOUR CODE HERE
    raise NotImplementedError()

YOUR ANSWER HERE

assert calc_scale(tp, tp*1) == 1.0
assert calc_scale(tp, tp*2) == 2.0
assert calc_scale(tp, tp*4) == 4.0

Compute translation

Translation is computed with the known scale $s$ and rotation $R$.

${\Large t=r_{com} - sRl_{com}}$

def calc_t(l_com, r_com, s, R):
    # YOUR CODE HERE
    raise NotImplementedError()

YOUR ANSWER HERE

# YOUR CODE HERE
raise NotImplementedError()

Application

Please estimate the transformation between point clouds left and right given in ply format. Apply the transformation to the left point cloud in order to reproduce the right cloud. Calculate the mean distance between the second cloud and the reproduced cloud.

# load data
_data = PlyData.read('points_left.ply').elements[0].data
left = np.stack((_data['x'], _data['y'], _data['z']), axis=1)
_data = PlyData.read('points_right.ply').elements[0].data
right = np.stack((_data['x'], _data['y'], _data['z']), axis=1) 

print(left)
print(right)

YOUR ANSWER HERE

Author:	Artem Leichter
Last modified:	2019-04-30