[Python] python numpy matrix multiplication for coordinate transformation gives a wrong result

Here is my code: I am performing a coordinate transformation on a set of 3D points. T is my 4×4 transformation matrix, which always satisfies the form [[R , t], [ 0, 1]](I am certain of this). points is the set of points I need to transform, with shape (M,3) where M is usually very large(1k~2k).

My problem is this: When I perform the coordinate transformation using Method 1 (homogeneous coordinates), the result is incorrect. Specifically, the last number of some of the transformed points (points_local_homogeneous) is not equal to 1, that's not correct. However, when I use Method 2, all the results are correct.

# Method 1: wrong result.
ones = np.ones((points.shape[0], 1))
points_homogeneous = np.hstack((points, ones))
points_local_homogeneous = (T @ points_homogeneous.T).T
points_local = points_local_homogeneous[:, :3]
# Method 2: Good results.
points_local= (T[:3, :3] @ points.T).T + T[:3, 3]


There is also a strange phenomenon that reflects the problem: When I first perform the matrix multiplication and then extract a point, the result is wrong. But when I extract the point first and then perform the matrix multiplication, the result is correct.

>> (T @ points_homogeneous.T).T[547]
array([-15.44923687, -0.03295934, -0.07585889, -15.45439878])
>> T @ points_homogeneous[547]
array([6.58938647, 3.43626129, 2.84996901, 1. ])


Can anyone help me understand the reason behind this? Any guidance would be greatly appreciated!

numpy version : 1.23.5

minimal reproducible example

import numpy as np


T =np.array([[-9.99997984e-01, 2.00338436e-03, 1.36517526e-04,2.30385575e+01],
[-2.00432316e-03, -9.99971609e-01, -7.26382636e-03,3.48879370e+00],
[ 1.21961414e-04, -7.26408534e-03, 9.99973609e-01,-5.29494007e-02],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,1.00000000e+00]])
points = np.random.random((1000,3))
print(points)


ones = np.ones((points.shape[0], 1))
points_homogeneous = np.hstack((points, ones))
points_local_homogeneous = (T @ points_homogeneous.T).T
print(points_local_homogeneous)
points_local = points_local_homogeneous[:, :3]
print(points_local)
points_local= (T[:3, :3] @ points.T).T + T[:3, 3]
print(points_local)


Output:

points:
[[0.70431115 0.18240672 0.33961428]
[0.03708955 0.67343087 0.82448419]
[0.56714298 0.73581627 0.6482321 ]
...
[0.57882963 0.04147515 0.05351834]
[0.67357367 0.02644866 0.19563735]
[0.86036017 0.27597764 0.63466743]]

points_local_homogeneous:
[[ 2.23346596e+01 3.30251359e+00 2.85416795e-01 1.00000000e+00]
[ 2.30029297e+01 2.80931870e+00 7.66625690e-01 1.00000000e+00]
[ 2.24729783e+01 2.74715294e+00 5.89989729e-01 1.00000000e+00]
...
[ 4.21157332e-01 -1.46142995e-03 9.03349144e-01 5.65885052e-01]
[ 3.26428767e-01 -1.54217777e-03 5.56317837e-01 4.18719365e-01]
[ 1.39730259e-01 -3.72909348e-03 6.30229075e+00 1.10249332e+00]]

points_local(Method 1):
[[ 2.23346596e+01 3.30251359e+00 2.85416795e-01]
[ 2.30029297e+01 2.80931870e+00 7.66625690e-01]
[ 2.24729783e+01 2.74715294e+00 5.89989729e-01]
...
[ 4.21157332e-01 -1.46142995e-03 9.03349144e-01]
[ 3.26428767e-01 -1.54217777e-03 5.56317837e-01]
[ 1.39730259e-01 -3.72909348e-03 6.30229075e+00]]

points_local(Method 2):
[[2.23346596e+01 3.30251359e+00 2.85416795e-01]
[2.30029297e+01 2.80931870e+00 7.66625690e-01]
[2.24729783e+01 2.74715294e+00 5.89989729e-01]
...
[2.24598194e+01 3.44577082e+00 3.36839801e-04]
[2.23650649e+01 3.45957466e+00 1.42572815e-01]
[2.21788386e+01 3.20648934e+00 5.79801482e-01]]


The last colomn of points_local_homogeneous should be all 1.0000, but it's not, and the result of Method 1 and Method 2 is different. I don't know why?

Continue reading...