Simple question: I used a translation and rotation matrix and camera intrinsics matrix to get a 3x4 matrix used to transform 3d points to 2d points (notated as Tform)
I transformed the point [10,-5,1] with the matrix by adding one to the end, and the new point is notated as newpoint.
Now I want to use the newpoint data to transform back to 3D space, where old_est should be equal to old.
I'm looking for the solution to plug into the XXX matrix in my code below
import numpy as np
Tform=np.array([[4000,0,-1600,-8000],[500,5000,868,-8000],[.5,0,.8,-8]])
old=np.array([10,-5,1,1])
newpoint=np.dot(Tform,old)
print(newpoint)
old_est=np.dot(XXX,np.append(newpoint,1))
print(old_est)
Add a 4th row to Tform with the values 0 0 0 1, i.e. the last row of an identity matrix:
>>> m = np.vstack(Tform, np.array([0,0,0,1]))
>>> m
array([[ 4.00e+03, 0.00e+00, -1.60e+03, -8.00e+03],
[ 5.00e+02, 5.00e+03, 8.68e+02, -8.00e+03],
[ 5.00e-01, 0.00e+00, 8.00e-01, -8.00e+00],
[ 0.00e+00, 0.00e+00, 0.00e+00, 1.00e+00]])
Note that you cannot use append because it also flattens the input arrays.
Observe that, when multiplied with old, the 4th component of the result is 1, i.e. the result is equal to np.append(newpoint, 1):
>>> np.dot(m, old)
array([ 3.0400e+04, -2.7132e+04, -2.2000e+00, 1.0000e+00])
----------
It follows that XXX is the inverse of this new matrix:
>>> XXX = np.linalg.inv(m)
>>> np.dot(XXX, np.append(newpoint, 1))
array([10., -5., 1., 1.])
-------------
And we get the components of old back.
Alternatively you can subtract the 4th column of Tform from newpoint and multiply the result with the inverse of the left 3x3 sub-matrix of Tform, but this is slightly fiddly so we might as well let numpy do more of the work :)