I am using Open3D to visualize some point clouds. I would like to add arrows that start and end at specific points. The arrows would visualize some things that I am working on. However, I have not found an easy way to add these arrows.
I have noticed that there's a function to create a Cartesian coordinate system, which is using arrows. So, it is possible to add arrows to the 3D visualization.
import open3d as o3d
# Create cartesian coordinate
FOR = o3d.geometry.TriangleMesh.create_coordinate_frame(
size=10, origin=[0,0,0])
# Visualize FOR
o3d.visualization.draw_geometries([FOR])
I was frustrated by not finding an easy way to create arrows within Open3D, and after some time struggling with it, I have come up with a solution.
import open3d as o3d
import numpy as np
def draw_geometries(pcds):
"""
Draw Geometries
Args:
- pcds (): [pcd1,pcd2,...]
"""
o3d.visualization.draw_geometries(pcds)
def get_o3d_FOR(origin=[0, 0, 0],size=10):
"""
Create a FOR that can be added to the open3d point cloud
"""
mesh_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(
size=size)
mesh_frame.translate(origin)
return(mesh_frame)
def vector_magnitude(vec):
"""
Calculates a vector's magnitude.
Args:
- vec ():
"""
magnitude = np.sqrt(np.sum(vec**2))
return(magnitude)
def calculate_zy_rotation_for_arrow(vec):
"""
Calculates the rotations required to go from the vector vec to the
z axis vector of the original FOR. The first rotation that is
calculated is over the z axis. This will leave the vector vec on the
XZ plane. Then, the rotation over the y axis.
Returns the angles of rotation over axis z and y required to
get the vector vec into the same orientation as axis z
of the original FOR
Args:
- vec ():
"""
# Rotation over z axis of the FOR
gamma = np.arctan(vec[1]/vec[0])
Rz = np.array([[np.cos(gamma),-np.sin(gamma),0],
[np.sin(gamma),np.cos(gamma),0],
[0,0,1]])
# Rotate vec to calculate next rotation
vec = Rz.T#vec.reshape(-1,1)
vec = vec.reshape(-1)
# Rotation over y axis of the FOR
beta = np.arctan(vec[0]/vec[2])
Ry = np.array([[np.cos(beta),0,np.sin(beta)],
[0,1,0],
[-np.sin(beta),0,np.cos(beta)]])
return(Rz, Ry)
def create_arrow(scale=10):
"""
Create an arrow in for Open3D
"""
cone_height = scale*0.2
cylinder_height = scale*0.8
cone_radius = scale/10
cylinder_radius = scale/20
mesh_frame = o3d.geometry.TriangleMesh.create_arrow(cone_radius=1,
cone_height=cone_height,
cylinder_radius=0.5,
cylinder_height=cylinder_height)
return(mesh_frame)
def get_arrow(origin=[0, 0, 0], end=None, vec=None):
"""
Creates an arrow from an origin point to an end point,
or create an arrow from a vector vec starting from origin.
Args:
- end (): End point. [x,y,z]
- vec (): Vector. [i,j,k]
"""
scale = 10
Ry = Rz = np.eye(3)
T = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
T[:3, -1] = origin
if end is not None:
vec = np.array(end) - np.array(origin)
elif vec is not None:
vec = np.array(vec)
if end is not None or vec is not None:
scale = vector_magnitude(vec)
Rz, Ry = calculate_zy_rotation_for_arrow(vec)
mesh = create_arrow(scale)
# Create the arrow
mesh.rotate(Ry, center=np.array([0, 0, 0]))
mesh.rotate(Rz, center=np.array([0, 0, 0]))
mesh.translate(origin)
return(mesh)
# Create a Cartesian Frame of Reference
FOR = get_o3d_FOR()
# Create an arrow from point (5,5,5) to point (10,10,10)
# arrow = get_arrow([5,5,5],[10,10,10])
# Create an arrow representing vector vec, starting at (5,5,5)
# arrow = get_arrow([5,5,5],vec=[5,5,5])
# Create an arrow in the same place as the z axis
arrow = get_arrow()
# Draw everything
draw_geometries([FOR,arrow])
As mentioned in paper 3D-RCNN, we can calculate the matrix between the align operation of 2 (unit)vectors, as shown in the formula below:
where 'r' is defined as the cross product of p and q.
And the '[r]x' are defined by a Skew-symmetric matrix, which can be found here.
So we can use this method to creat needed arrow or cylinder: just use the previous matrix as a rotation of the target and align the center.
The demo can be realized as following way(based on open3d 0.9.0):
import numpy as np
import open3d as o3d
def get_cross_prod_mat(pVec_Arr):
# pVec_Arr shape (3)
qCross_prod_mat = np.array([
[0, -pVec_Arr[2], pVec_Arr[1]],
[pVec_Arr[2], 0, -pVec_Arr[0]],
[-pVec_Arr[1], pVec_Arr[0], 0],
])
return qCross_prod_mat
def caculate_align_mat(pVec_Arr):
scale = np.linalg.norm(pVec_Arr)
pVec_Arr = pVec_Arr/ scale
# must ensure pVec_Arr is also a unit vec.
z_unit_Arr = np.array([0,0,1])
z_mat = get_cross_prod_mat(z_unit_Arr)
z_c_vec = np.matmul(z_mat, pVec_Arr)
z_c_vec_mat = get_cross_prod_mat(z_c_vec)
if np.dot(z_unit_Arr, pVec_Arr) == -1:
qTrans_Mat = -np.eye(3, 3)
elif np.dot(z_unit_Arr, pVec_Arr) == 1:
qTrans_Mat = np.eye(3, 3)
else:
qTrans_Mat = np.eye(3, 3) + z_c_vec_mat + np.matmul(z_c_vec_mat,
z_c_vec_mat)/(1 + np.dot(z_unit_Arr, pVec_Arr))
qTrans_Mat *= scale
return qTrans_Mat
if __name__ == "__main__":
z_unit_Arr = np.array([0,0,1])
begin = [1, 0, 0]
end = [1.6, 0.4, 0.8]
vec_Arr = np.array(end) - np.array(begin)
vec_len = np.linalg.norm(vec_Arr)
mesh_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.6, origin=[0, 0, 0])
mesh_arrow = o3d.geometry.TriangleMesh.create_arrow(
cone_height= 0.2 * vec_len,
cone_radius= 0.06 * vec_len,
cylinder_height= 0.8 * vec_len,
cylinder_radius= 0.04 * vec_len
)
mesh_arrow.paint_uniform_color([1,0,1])
mesh_arrow.compute_vertex_normals()
mesh_sphere_begin = o3d.geometry.TriangleMesh.create_sphere(radius=0.1, resolution= 20)
mesh_sphere_begin.translate(begin)
mesh_sphere_begin.paint_uniform_color([0,1,1])
mesh_sphere_begin.compute_vertex_normals()
mesh_sphere_end = o3d.geometry.TriangleMesh.create_sphere(radius=0.1, resolution= 20)
mesh_sphere_end.translate(end)
mesh_sphere_end.paint_uniform_color([0,1,1])
mesh_sphere_end.compute_vertex_normals()
# mesh_arrow,
o3d.visualization.draw_geometries(
geometry_list= [mesh_frame, mesh_arrow, mesh_sphere_begin, mesh_sphere_end],
window_name= "before", width= 800, height= 600
)
rot_mat = caculate_align_mat(vec_Arr)
mesh_arrow.rotate(rot_mat, center = False)
o3d.visualization.draw_geometries(
geometry_list= [mesh_frame, mesh_arrow, mesh_sphere_begin, mesh_sphere_end],
window_name= "after rotate", width= 800, height= 600
)
mesh_arrow.translate(np.array(begin)) # 0.5*(np.array(end) - np.array(begin))
o3d.visualization.draw_geometries(
geometry_list= [mesh_frame, mesh_arrow, mesh_sphere_begin, mesh_sphere_end],
window_name= "after translate", width= 800, height= 600
)
Related
I'm trying to perform complex rotations of a point around the origin in a 3d space. I know my starting point and the target ending point. I'm trying to find the rotation matrix and apply it to the starting point to see if I get the target point. I think my rotation matrix calculation is incorrect but I dont know why. I based it off of this answer.
import matplotlib.pyplot as plt
import numpy as np
import math
def get_rot_matrix(A, B):
assert A.shape == B.shape
v = np.cross(A, B)
s = np.linalg.norm(v)
c = np.dot(A, B)
vx = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
r = np.eye(3) + vx + (np.dot(vx, vx) * (1-c)/(s**2))
return r
fig = plt.figure(figsize=(8,8))
ax = fig.add_subplot(111, projection='3d')
# ORIGIN
ax.scatter(0,0,0)
point = np.array([3,2,5])
# STARTING
ax.scatter(point[0], point[1], point[2])
# vector magnitude (pythagreon)
length = math.sqrt(sum(a**2 for a in point))
# TARGET POINT
ax.scatter(0, 0, length)
# calc rotation matrix
R = get_rot_matrix(np.array(point), np.array([0, 0, length]))
# RESULT
result_point = np.dot(R, point)
print("Result:")
print(result_point)
ax.scatter(result_point[0], result_point[1], result_point[2])
plt.xlabel("X")
plt.ylabel("Y")
plt.legend(["origin", "start", "target", "result"])
ax.set_xlim3d(-1,6)
ax.set_ylim3d(-1,6)
ax.set_zlim3d(-1,6)
plt.show()
Result point does not appear on the chart:
Result:
[-1.06581410e-14 -2.30926389e-14 2.34247732e+02]
I have two shapes, a rectangle and a parallelogram that signify two gantry systems. The one gantry system has a camera on it and can detect the position of the other gantry system as it sits above. I cannot via a series of transforms (translate, rotate, shear x, shear y, translate) get it even remotely close to fitting to the system 1. Could I please get some pointers/insight as to what I am doing wrong?
I've tested each transform with a unit vector so I know the math works. I suspect either I am using the incorrect angles(using the same on the unit vectors though), there are linearity issues where it is not quite linear and therefore transforms wont work (this also seems unlikely due to the physical nature), or most likely my order of operations are incorrect.
from matplotlib import pyplot as plt
import numpy as np
from mpl_toolkits.axes_grid1.inset_locator import TransformedBbox, BboxPatch, BboxConnector
def get_angle(array, array_2, side=3):
if side == 0:
# Get start and end points from array
vector = array[1] - array[0]
# Get start and end points from array
vector_2 = array_2[1] - array_2[0]
elif side == 1:
# Get start and end points from array
vector = array[2] - array[1]
# Get start and end points from array
vector_2 = array_2[2] - array_2[1]
elif side == 2:
# Get start and end points from array
vector = array[2] - array[3]
# Get start and end points from array
vector_2 = array_2[2] - array_2[3]
elif side == 3:
# Get start and end points from array
vector = array[3] - array[0]
# Get start and end points from array
vector_2 = array_2[3] - array_2[0]
# Calculate unit vectors
dot = vector[0] * vector_2[0] + vector[1] * vector_2[1] # dot product between [x1, y1] and [x2, y2]
det = vector[0] * vector_2[1] - vector[1] * vector_2[0] # determinant
angle = np.arctan2(det, dot) # atan2(y, x) or atan2(sin, cos)
return angle
def shear_trans_x(coords, phi):
shear_x = np.array([[1, np.tan(phi), 0],
[0, 1, 0],
[0, 0, 1]])
coords = np.append(coords, np.ones((coords.shape[0], 1)), axis=1)
resultant = coords # shear_x.T
return resultant[:, 0:2]
def shear_trans_y(coords, psi):
shear_y = np.array([[1, 0, 0],
[np.tan(psi), 1, 0],
[0, 0, 1]])
coords = np.append(coords, np.ones((coords.shape[0], 1)), axis=1)
resultant = coords # shear_y.T
return resultant[:, 0:2]
def translate(coordinates, offset):
coordinates = np.append(coordinates, np.ones((coordinates.shape[0], 1)), axis=1)
a = np.array([[1, 0, offset[0]],
[0, 1, offset[1]],
[0, 0, 1 ]])
result = coordinates # a.T
return result[:, 0:2]
def rotate(coords, theta, origin=[0,0]):
cos = np.cos(theta)
sin = np.sin(theta)
a = np.array([[cos, -sin, 0],
[sin, cos, 0],
[0, 0, 1]])
if np.all(origin == [0, 0]):
coords = np.append(coords, np.ones((coords.shape[0], 1)), axis=1)
result = coords # a.T
return result[:, 0:2]
else:
coords = translate(coords, -origin)
coords = rotate(coords, theta, origin=[0, 0])
coords = translate(coords, origin)
return coords
def mark_inset(parent_axes, inset_axes, loc1a=1, loc1b=1, loc2a=2, loc2b=2, **kwargs):
'''
draw a bbox of the region of the inset axes in the parent axes and
connecting lines between the bbox and the inset axes area
loc1, loc2 : {1, 2, 3, 4}
'''
rect = TransformedBbox(inset_axes.viewLim, parent_axes.transData)
p1 = BboxConnector(inset_axes.bbox, rect, loc1=loc1a, loc2=loc1b, **kwargs)
inset_axes.add_patch(p1)
p1.set_clip_on(False)
p2 = BboxConnector(inset_axes.bbox, rect, loc1=loc2a, loc2=loc2b, **kwargs)
inset_axes.add_patch(p2)
p2.set_clip_on(False)
pp = BboxPatch(rect, fill=False, **kwargs)
parent_axes.add_patch(pp)
return pp, p1, p2
if __name__ == '__main__':
# calibration data
gantry_1_coords = np.array([[169.474, 74.4851], [629.474, 74.4851], [629.474, 334.4851], [169.474, 334.4851]])
gantry_2_coords_error = np.array([[-0.04, 0.04], [-0.04, -0.31], [0.76, -0.57], [1.03, 0.22]])
# gantry_2_coords_error = np.array([[0.13, 0.04], [-0.13, -0.75], [0.31, -0.93], [0.58, -0.31]])
# add error to gantry 1 coords
gantry_2_coords = gantry_1_coords + gantry_2_coords_error
# append first point to end for plotting to display a closed box
gantry_1_coords = np.append(gantry_1_coords, np.array([gantry_1_coords[0]]), axis=0)
gantry_2_coords = np.append(gantry_2_coords, np.array([gantry_2_coords[0]]), axis=0)
# get length of diagonal direction
magnitude = np.linalg.norm(gantry_1_coords[0] - gantry_1_coords[2])
magnitude_gantry_2 = np.linalg.norm(gantry_2_coords[0] - gantry_2_coords[2])
# translate to gantry_1 first position
translated_gantry_2 = translate(gantry_2_coords, (gantry_1_coords[0] - gantry_2_coords[0]))
print('translation_offset_1', ' = ', gantry_1_coords[0] - gantry_2_coords[0])
# rotate gantry_2 to gantry_1
theta = get_angle(translated_gantry_2, gantry_1_coords, side=0)
rotate_gantry_2_coords = rotate(translated_gantry_2, theta, translated_gantry_2[0])
print('rotation angle', ' = ', theta)
# un-shear x axis gantry_2
shear_phi = get_angle(rotate_gantry_2_coords, gantry_1_coords, side=3)
sheared_x_gantry_2 = shear_trans_x(rotate_gantry_2_coords, shear_phi)
print('shear x angle', ' = ', shear_phi)
# un-shear y axis gantry_2
shear_psi = get_angle(sheared_x_gantry_2, gantry_1_coords, side=2)
sheared_y_gantry_2 = shear_trans_y(sheared_x_gantry_2, shear_psi)
print('shear y angle', ' = ', shear_psi)
# translate to gantry_1 first position
final_gantry_2_coords = translate(sheared_y_gantry_2, (gantry_1_coords[0] - sheared_y_gantry_2[0]))
print('translation_offset_2', ' = ', gantry_1_coords[0] - sheared_y_gantry_2[0])
# create exaggerated errors for plotting
ex_gantry_2_coords = (gantry_2_coords - gantry_1_coords) * 50 + gantry_2_coords
ex_gantry_2_final_coords = (final_gantry_2_coords - gantry_1_coords) * 50 + final_gantry_2_coords
# separate out x & y components for plotting
gantry_1_x, gantry_1_y = gantry_1_coords.T
gantry_2_x, gantry_2_y = ex_gantry_2_coords.T
gantry_2_final_x, gantry_2_final_y = ex_gantry_2_final_coords.T
# plot results
fig, ax = plt.subplots()
ax.plot(gantry_1_x, gantry_1_y, color='black', linestyle='--', label='gantry_1')
ax.plot(gantry_2_x, gantry_2_y, color='blue', linestyle='--', label='gantry_2 original')
ax.plot(gantry_2_final_x, gantry_2_final_y, color='red', linestyle='--', label='gantry_2 transformed')
# get legend lines and labels from center graph
lines, labels = ax.get_legend_handles_labels()
fig.legend(lines, labels)
plt.show()
# print('gantry 1 positions: ', gantry_1_coords)
# print('transformed gantry 2 positions: ', final_gantry_2_coords)
Fixing existing code
In terms of the existing code, I applied the transformations one by one, and I think you're missing a negative sign here:
sheared_x_gantry_2 = shear_trans_x(rotate_gantry_2_coords, -shear_phi)
# ^--- here
After applying that, the graph looks better:
Least squares fit
However, I think this is the wrong general approach. For example, when you fix the shear, that's going to break the translation and rotation, at least a little bit. You can repeatedly apply the fixes, and converge on the right answer, but there's a better way.
Instead, I would suggest finding a least-squares fit for the transformation matrix, rather than building up a bunch of rotation and shear matrices. Numpy has a function that will do this.
def add_bias_term(matrix):
return np.append(np.ones((matrix.shape[0], 1)), matrix, axis=1)
x, _, _, _ = np.linalg.lstsq(add_bias_term(gantry_2_coords), gantry_1_coords, rcond=None)
final_gantry_2_coords = add_bias_term(gantry_2_coords) # x
This is both a heck of a lot shorter, and produces a better fit to boot:
And here is the matrix that it finds:
array([[ 0.19213806, -0.37107611],
[ 1.00028902, 0.00123954],
[-0.00359818, 1.00014869]])
(Note that the first row is the bias term.)
Although, the fit is not perfect. Here are the residuals:
array([[-0.06704727, -0.10997465], # point 0
[ 0.06716097, 0.11016114], # point 1
[-0.06720015, -0.1102254 ], # point 2
[ 0.06708645, 0.11003891]]) # point 3
Unfortunately, this remaining error is nonlinear, by definition. (If there were an affine matrix which reduced the error better, lstsq would have found it.)
Adding nonlinearity
Eyeballing the residuals, the error goes in one direction when both x and y are large, and in the other direction when only one of x or y are large. That suggests to me that you need an interaction term. In other words, you need to preprocess the input matrix so that it has a column with X, a column with Y, and a column with X*Y.
The code to do that looks like this:
def add_bias_term(matrix):
return np.append(np.ones((matrix.shape[0], 1)), matrix, axis=1)
def add_interaction(matrix):
inter = (matrix[:, 0] * matrix[:, 1]).reshape(matrix.shape[0], 1)
return np.append(inter, matrix, axis=1)
x, _, _, _ = np.linalg.lstsq(add_bias_term(add_interaction(gantry_2_coords)), gantry_1_coords, rcond=None)
final_gantry_2_coords = (add_bias_term(add_interaction(gantry_2_coords)) # x)
And the graph for that looks like this:
And that's close enough that the two graphs are right on top of each other.
I want to simulate gyroscope readings (angular velocity in local frame) by supplied vectors.
I have this vectors (x,y,z) with sampling 1s:
[0, 0, 1000]
[469.47156279, 0.0, 882.94759286]
[981.62718345, 0.0, 190.80899538]
Rotating Z by 28 degrees by Y axis and then by 51 degrees again around Y.
Vectors looks like this:
I am getting quaternions describing relative rotations using
scipy.spatial.transform.Rotation.align_vectors
In a form that
1. vector_orientation = quaternion_1
2. vector_orientation = quaternion_2 * quaternion_1
3. vector_orientation = quaternion_3 * quaternion_2 * quaternion_1
I found this How to compute angular velocity using numpy-quaternion question, so I tried to implement suggested code for calculating angular velocity between orientation 2 and 3:
# quaternion_3 is relative to quaternion_2, so I guess I don't need quaternion difference
delta_q = quaternion_3
delta_q_len = np.linalg.norm(delta_q[1:])
delta_q_angle = 2*np.arctan2(delta_q_len, delta_q[0])
w = delta_q[1:] * delta_q_angle * 1
I got some velocities - [0.0, 0.38320564, 0.0].
However if I want to check the result by applying it to the quaternion_2 by obtaining derivative using this equation:
So in Python:
omega = np.array([[0, -w[0], -w[1], -w[2]],
[w[0], 0, w[2], -w[1]],
[w[1], -w[2], 0, w[0]],
[w[2], w[1], -w[0], 0]])
quaternion_derivative = 0.5 * omega # quaternion_2
And integrate it with
quaternion_result = (2 * quaternion_derivative / quaternion_2).normalized()
Problem is quaternion_result != quaternion_3. Does anybody has some idea what I am doing wrong?
Edit:
Adding (not)working example. Also changed angles for better demonstration. I am using https://github.com/moble/quaternion for work with quaternions:
import numpy as np
from scipy.spatial.transform import Rotation as R
import quaternion as qt
def get_quaternion_rotation(v1, v2):
rotation,_ = R.align_vectors([v1], [v2])
qt = rotation.as_quat()
return np.array([qt[3], qt[0], qt[1], qt[2]])
def rotate_vector(vector, axis, angle):
# normalize
unit_axis = axis/np.linalg.norm(axis)
angle_radians = np.radians(angle)
rotation_vector = angle_radians * unit_axis
rotation = R.from_rotvec(rotation_vector)
return rotation.apply(vector)
# 1. vector is just 1000 in Z
vector_1 = np.array([0, 0, 1000])
# 2. vector rotates vector_1 28 degrees by Y
vector_2 = rotate_vector(vector_1, [0,1,0], 28)
# 3. vector rotates vector_2 51 degrees by Y
vector_3 = rotate_vector(vector_2, [0,1,0], 51)
# rotations between vectors
quaternion_1 = get_quaternion_rotation([0,0,0], vector_1)
quaternion_2 = get_quaternion_rotation(vector_2, vector_1)
quaternion_3 = get_quaternion_rotation(vector_3, vector_2)
delta_q = quaternion_3
delta_q_len = np.linalg.norm(delta_q[1:])
delta_q_angle = 2 * np.arctan2(delta_q_len, delta_q[0])
w = delta_q[1:] * delta_q_angle * 1
omega = np.array([[0, -w[0], -w[1], -w[2]],
[w[0], 0, w[2], -w[1]],
[w[1], -w[2], 0, w[0]],
[w[2], w[1], -w[0], 0]])
qt_derivative = 0.5 * omega # quaternion_2
# converting to quaternion type for normalizing
qt_derivative = qt.quaternion(qt_derivative[0], qt_derivative[1], qt_derivative[2], qt_derivative[3]).normalized()
# apply derivative to quaternion_2
qt_result = (2 * qt_derivative / qt.quaternion(quaternion_2[0], quaternion_2[1], quaternion_2[2], quaternion_2[3])).normalized()
print(f"Quaternion 3 = {quaternion_3}")
print(f"Quaternion result = {qt.as_float_array(qt_result)}")
rotation_quaternion_3 = R.from_quat([quaternion_3[1], quaternion_3[2], quaternion_3[3], quaternion_3[0]])
print(f"Applied rotation from quaternion_3 to vector_2: {rotation_quaternion_3.apply(vector_2)}")
rotation_result = R.from_quat([qt_result.x, qt_result.y, qt_result.z, qt_result.w])
print(f"Applied rotation from quaternion result to vector_2: {rotation_result.apply(vector_2)}")
Result is:
Quaternion 3 = [0.90258528 0. 0.4305111 0. ]
Quaternion result = [-2.77555756e-17 -2.76809689e-17 -1.00000000e+00 -1.11022302e-16]
Applied rotation from quaternion_3 to vector_2: [981.62718345 0. 190.80899538]
Applied rotation from quaternion result to vector_2: [-4.69471563e+02 2.22044605e-13 -8.82947593e+02]
I am trying to get blender render depth map of an object and then moving it to overlay the original object. Currently I have no issue with rendering the object and extracting it into it's place.
However, I am stuck when trying to position the object into it's original position.
I'm trying to apply inverse camera world matrix to the rendered pointcloud (in blue). Unfortunately, when I apply said camera inverse it doesn't appear nowhere near where I'd expect (in red).
I have attached the entirety of code that I have to replicate this behaviour. I would appreciate it if someone would point me to the right matrix that I should be multiplying the point cloud by.
from mathutils import Vector, Quaternion, Euler, Matrix
import numpy as np
import bpy
def main_script():
clear_scene()
prepare_views()
tmp_path = "/tmp/tmp_render.exr"
scene = get_scene("Scene")
camera = create_camera("Camera")
camera.rotation_euler = Euler([np.pi * 0.5, 0, np.pi * 0.5], "XYZ")
camera.location = Vector([4.5, 0, 1])
bpy.ops.mesh.primitive_monkey_add(
location=(0, 0, 1), rotation=(0, 0, np.pi*0.5), size=1.0)
_w, _h = 640, 480
update_scene()
init_rendering(scene, camera, width=640, height=480)
update_scene()
matrix_K = get_calibration_matrix_K_from_blender(scene, camera.data)
_fy, _fx = matrix_K[0][0], matrix_K[1][1]
_cy, _cx = matrix_K[0][2], matrix_K[1][2]
scene.render.filepath = tmp_path
bpy.ops.render.render(write_still=True)
depth = read_exr(tmp_path, "R")["R"]
depth = np.reshape(convert_to_numpy(depth), [_h, _w])
exr_cloud = depth_to_cloud(
_w, _h, _fx, _fy, _cx, _cy, depth)
exr_cloud = np.reshape(exr_cloud, [-1, 3])
exr_cloud = exr_cloud[(exr_cloud[..., 2] < 100) & (exr_cloud[..., 2] > 0)]
matrix = np.reshape(camera.matrix_world, [4, 4])
matrix = np.linalg.inv(matrix) # why doesn't this place the depth properly
vertices = np.ones([exr_cloud.shape[0], 4], dtype=np.float32)
vertices[:, 0:3] = exr_cloud
vertices = np.array(
[matrix # vertex for vertex in vertices], dtype=np.float32)
vertices = vertices[..., :3]
create_mesh("Suzanne_EXR", exr_cloud, [])
create_mesh("SuzanneT_EXR", vertices, [])
"""
utilities methods required to run the script
"""
def clear_scene():
for scene in bpy.data.scenes:
for obj in scene.objects:
bpy.context.collection.objects.unlink(obj)
def read_exr(path, channels):
import OpenEXR as _OpenEXR
import Imath as _Imath
file = _OpenEXR.InputFile(path)
FLOAT = _Imath.PixelType(_Imath.PixelType.FLOAT)
results = {}
for ch in channels:
results[ch] = file.channel(ch, FLOAT)
file.close()
return results
def convert_to_numpy(data):
import array as _array
return np.array(_array.array("f", data).tolist())
def update_scene():
dg = bpy.context.evaluated_depsgraph_get()
dg.update()
def prepare_views():
preferences = bpy.context.preferences
preferences.view.show_tooltips_python = True
preferences.view.show_developer_ui = True
preferences.view.render_display_type = "NONE"
def init_rendering(scene, camera, width=None, height=None):
def set_rendering_settings(camera, scene, width=640, height=480):
image_settings = scene.render.image_settings
image_settings.file_format = "OPEN_EXR"
image_settings.use_zbuffer = True
scene.render.resolution_x = width
scene.render.resolution_y = height
# scene.render.use_antialiasing = False
scene.use_nodes = True
scene.camera = camera
node_tree = scene.node_tree
nodes = node_tree.nodes
node_render_layers = nodes["Render Layers"]
node_composite = nodes["Composite"]
node_tree.links.clear()
node_tree.links.new(
node_render_layers.outputs["Depth"], node_composite.inputs["Image"])
set_rendering_settings(camera, scene)
def get_scene(name): return bpy.data.scenes[name]
def create_camera(name):
camera = bpy.data.cameras.new(name)
camera.lens = 50
obj = bpy.data.objects.new(name, camera)
bpy.context.collection.objects.link(obj)
return obj
# ---------------------------------------------------------------
# 3x4 P matrix from Blender camera
# ---------------------------------------------------------------
# Build intrinsic camera parameters from Blender camera data
#
# See notes on this in
# blender.stackexchange.com/questions/15102/what-is-blenders-camera-projection-matrix-model
def get_calibration_matrix_K_from_blender(scene, camera):
from mathutils import Matrix as _Matrix
f_in_mm = camera.lens
resolution_x_in_px = scene.render.resolution_x
resolution_y_in_px = scene.render.resolution_y
scale = scene.render.resolution_percentage / 100
sensor_width_in_mm = camera.sensor_width
sensor_height_in_mm = camera.sensor_height
pixel_aspect_ratio = scene.render.pixel_aspect_x / scene.render.pixel_aspect_y
if (camera.sensor_fit == 'VERTICAL'):
# the sensor height is fixed (sensor fit is horizontal),
# the sensor width is effectively changed with the pixel aspect ratio
s_u = resolution_x_in_px * scale / sensor_width_in_mm / pixel_aspect_ratio
s_v = resolution_y_in_px * scale / sensor_height_in_mm
else: # 'HORIZONTAL' and 'AUTO'
# the sensor width is fixed (sensor fit is horizontal),
# the sensor height is effectively changed with the pixel aspect ratio
pixel_aspect_ratio = scene.render.pixel_aspect_x / scene.render.pixel_aspect_y
s_u = resolution_x_in_px * scale / sensor_width_in_mm
s_v = resolution_y_in_px * scale * pixel_aspect_ratio / sensor_height_in_mm
# Parameters of intrinsic calibration matrix K
alpha_u = f_in_mm * s_u
alpha_v = f_in_mm * s_v
u_0 = resolution_x_in_px * scale / 2
v_0 = resolution_y_in_px * scale / 2
skew = 0 # only use rectangular pixels
K = _Matrix(
((alpha_u, skew, u_0),
(0, alpha_v, v_0),
(0, 0, 1)))
return K
def create_mesh(name, vertices, faces):
import bmesh as _bmesh
mesh = bpy.data.meshes.new("Mesh_%s" % name)
mesh.from_pydata(vertices, [], faces)
mesh.update()
obj = bpy.data.objects.new(name, mesh)
bpy.context.collection.objects.link(obj)
bm = _bmesh.new()
bm.from_mesh(mesh)
bm.to_mesh(mesh)
bm.free()
return obj
def depth_to_cloud(w, h, fx, fy, cx, cy, depth):
from numpy import concatenate as _concat
from numpy import indices as _indices
from numpy import newaxis as _newaxis
indices = _indices(depth.shape)
indices_y, indices_x = indices
ys, xs, zs = \
(indices_y - cy) * depth / fy, \
(indices_x - cx) * depth / fx, \
depth
points = _concat([xs[..., _newaxis], ys[..., _newaxis],
zs[..., _newaxis]], axis=2)
return points
if __name__ == "__main__":
raise main_script()
The problem was compound, first I needed to replace my transformed vertex calculation from instead using inverse camera world matrix, to negatively scaled camera world matrix like so
matrix_cam = np.reshape(camera.matrix_world, [4, 4])
mat_scale = np.array(Matrix.Scale(-1, 4))
matrix = matrix_cam # mat_scale
vertices = np.ones([exr_cloud.shape[0], 4], dtype=np.float32)
vertices[:, 0:3] = exr_cloud
vertices = np.array(
[matrix # vertex for vertex in vertices], dtype=np.float32)
vertices = vertices[..., :3]
Additionally, there was an issue with depth decoding which caused the point cloud to be deformed, fixed like so
ys, xs, zs = \
(indices_y - cx) * depth / fx, \
(indices_x - cy) * depth / fy, \
depth
I want to implement the DLT algorithm and I have 6 object points (X,Y,Z,1).T and 6 image points (u,v,1).T , that are the projection of the object points to an image plane.
So before implementing the DLT I have to normalize data.
More specific I found that I have to do the following:
2D image points should be normalized so that their centroid is at the origin and their root-mean-square distance from the origin is sqrt(2)
Any idea how I can do that in python?
Taking 6 image points for two images, one original and other warped, then:
x1 = np.array([202,202,500,523,530,522])
y1 = np.array([459,473,403,403,405,434])
x2 = np.array([283,285,526,544,552,550])
y2 = np.array([482,494,371,367,365,392])
img1 = np.column_stack((x1,y1))
img2= np.column_stack((x2,y2))`
def normalise(img):
'''
input:img = image points that we want to normalize
return:Tr = Transformation that normalises the points
normalised_points = Points normalise by Tr
'''
s = np.sqrt(2)/((1/25)*np.sum((np.sqrt(abs(img - np.mean(img,axis=0))**2))))
m = np.mean(img,0)
normalised_points = np.zeros((25,3))
Tr = np.array([[s, 0, m[0]], [0, s, m[1]], [0, 0, 1]])
for i in range(img.shape[0]):
normalised_points[i][0] = s*img[i][0] + m[0]
normalised_points[i][1] = s*img[i][1] + m[1]
normalised_points[i][2] = 1
return Tr, normalised_points
Tr1,normalised_X = normalise(img1)
Ref