I am a MATLAB user, and trying to convert a rotation/translation code into Python:
from functools import reduce
import numpy as np
import matplotlib as plt
from math import pi as PI
from math import sin as sin
from math import cos as cos
# plt.close('all')
# coordinate in old system
x0 = np.array([ [10],
[5],
[0],])
x = x0
# translation
# origin of old coordinate system in relative to the new coordinate system
T = np.array([ [10],
[0],
[0]])
## rotation
# rotate alpha, beta, and gamme about x-axis, y-axis, and z- axis,
# respectively, in relative to the new coordinate system
alpha = 0
beta = 0
gamma = PI/2
# rotation matrices
Rx = np.array([ [1, 0, 0],
[0, cos(alpha), -sin(alpha)],
[0, sin(alpha), cos(alpha)] ])
Ry = np.array([ [cos(beta), 0, sin(beta)],
[0, 1, 0],
[-sin(beta), 0, cos(beta)]])
Rz = np.array([ [cos(gamma), -sin(gamma), 0],
[sin(gamma), cos(gamma), 0],
[0, 0, 1] ])
# R = Rz*Ry*Rx
R = reduce(np.dot, [Rz, Ry, Rx])
RT = np.array([ [R, T],
[0, 0, 0, 1]])
The last line trying to create a homogeneous 4X4 matrix RT
by stacking a 3X3 rotation matrix R
and 3X1 translation vector T
. The [0,0,0,1] at the bottom of the matrix is just to turn the transformation matrix RT
into a homogeneous format.
The code have problem stack the R
, T
and [0,0,0,1]
together. I am trying understand what is the proper way to do it? Thanks!!
Try
intermediate = np.concatenate((R, T.T), axis=1)
RT = np.concatenate((intermediate, np.array([0, 0, 0, 1])), axis=0)