我在空间中有两个 3D 对象，我想将点从一个对象复制到另一个对象。问题是这些对象不共享一个共同的坐标系，我必须进行坐标转换。我有两个对象的局部变换矩阵，我也可以访问世界变换矩阵。我知道要使用这些转换矩阵进行一些计算，但我不知道如何。
如果我将它复制到另一个对象（或其坐标系）中，如何转换第一个对象中的一个点，使其具有相同的位置（相对于世界坐标）？
谢谢
问问题
791 次
2 回答
1
好吧，您必须应用您拥有的转换运算符。例如，极坐标 (r, t) 和笛卡尔坐标 (x, y) 之间的关系定义为：
x = rcost
y = rsint
于 2012-10-23T14:05:41.180 回答
1
我晚了大约 9 年，但我必须解决同样的问题。
使用齐次变换矩阵。为此，首先使用描述局部 cs 的单位向量（使用相对于全局 cs 的值）计算 4x4 旋转矩阵。然后计算一个类似的 4x4 平移矩阵。那么变换矩阵就是平移矩阵和旋转矩阵的点积。计算齐次变换矩阵的逆并不是那么简单。我发现伊利诺伊大学机器人课堂笔记帮助我理解了需要什么。当然，细节真的会让你大吃一惊，所以我觉得有必要将细节呈现为 python 代码。
让我们调用以下代码“transform_coord_sys.py”：
# Functions to convert points etc from one coordinate system to another
#
# These functions are based on a 4x4 homogeneous matrices or
# homogeneous coordinates introduced by August Ferdinand Möbius
# See: https://en.wikipedia.org/wiki/Homogeneous_coordinates
#
# Other work I found useful in developing this code includes:
# University of Illinois Robotics course http://motion.cs.illinois.edu/RoboticSystems/CoordinateTransformations.html
import numpy as np
def unit_vector(xyz_array):
"""
Compute unit vector of vector from origin to specified xyz point
:param xyz_array: numpy array of length 3. x = 1st element, y = 2nd element, z = 3rd element
:return: xyz_unit : unit vector (ie ijk) in specified direction
"""
num_xyz = len(xyz_array)
if num_xyz != 3:
raise ValueError('Input array to "unit_vector" must have length of 3.')
xyz_unit = xyz_array / np.linalg.norm(xyz_array)
return xyz_unit
def create_rotation_matrix_from_points(local_origin, local_x_dir, local_xy_plane):
"""
Create a 4x4 homogeneous rotation matrix
:param local_origin: np_array of x,y,z wrt global cs
:param local_x_dir: np_array of x,y,z point. vector from local_origin to this point defines X
:param local_xy_plane: point in xy plane
:return: rot_matrix: homogeneous rotation matrix
"""
local_x_vec = local_x_dir - local_origin
local_unit_x = unit_vector(local_x_vec)
local_xy_vec = local_xy_plane - local_origin
local_z_vec = np.cross(local_x_vec, local_xy_vec)
local_unit_z = unit_vector(local_z_vec)
local_y_vec = np.cross(local_z_vec, local_x_vec)
local_unit_y = unit_vector(local_y_vec)
# print('local_unit_x = {}'.format(local_unit_x))
# print('local_unit_y = {}'.format(local_unit_y))
# print('local_unit_z = {}'.format(local_unit_z))
rot_matrix = np.zeros((4, 4))
rot_matrix[3, 3] = 1.0
rot_matrix[0:3, 0] = local_unit_x
rot_matrix[0:3, 1] = local_unit_y
rot_matrix[0:3, 2] = local_unit_z
determinant = np.linalg.det(rot_matrix)
assert np.isclose(determinant, 1.0)
# print('rot_matrix = \n{}'.format(rot_matrix))
# print('determinant = {}\n'.format(determinant))
return rot_matrix
def create_translation_matrix_from_point(point, origin=np.zeros(3)):
"""
Create a 4x4 homogeneous translation matrix
:param point: np_array of x,y,z wrt global cs
:param origin: np_array of x,y,z point. vector from local_origin to this point defines X
:return: translation_matrix : homogeneous translation matrix
"""
translation_matrix = np.identity(4)
deltas = point - origin
translation_matrix[0:3, 3] = deltas
# print('translation_matrix = \n{}'.format(translation_matrix))
return translation_matrix
def invert_homogeneous_transformation_matrix(transformation_matrix):
"""
Invert a homogeneous transformation matrix
:param transformation_matrix: homogeneous transformation matrix
:return: inverse_transform: inverted matrix
"""
rot_matrix = transformation_matrix[0:3, 0:3]
translation = transformation_matrix[0:3, 3]
# for orthogonal arrays the transpose is equal to the inverse
rot_inverse = rot_matrix.transpose()
trans_inv = -rot_inverse.dot(translation)
inverse_transform = np.identity(4)
inverse_transform[0:3, 0:3] = rot_inverse
inverse_transform[0:3, 3] = trans_inv
return inverse_transform
def calculate_homogeneous_transforms(local_origin, local_x_dir, local_xy_plane):
rot_matrix_4x4 = create_rotation_matrix_from_points(local_origin, local_x_dir, local_xy_plane)
translation_matrix = create_translation_matrix_from_point(local_origin)
# This is the key step showing how the transformation_matrix is created
# using matrix multiplication of the translation and rotation matrices.
# Order is CRITICAL:
# translation_matrix.dot(rot_matrix_4x4) IS NOT EQUAL TO rot_matrix_4x4.dot(translation_matrix)
# (except it trivial cases where it provides the correct answer but is still wrong)
transformation_matrix = translation_matrix.dot(rot_matrix_4x4)
inverse_transform = invert_homogeneous_transformation_matrix(transformation_matrix)
return transformation_matrix, inverse_transform
def test_pure_rotation():
print('{}'.format('-'*80))
print('testing test_pure_rotation')
local_origin = np.asarray([0.0, 0.0, .0])
local_x_dir = np.asarray([0.0, 1.0, 0.0])
local_xy_plane = np.asarray([-1.0, 1.0, 0.0])
print(' local_origin = {}'.format(local_origin))
print(' local_x_dir = {}'.format(local_x_dir))
print(' local_xy_plane = {}'.format(local_xy_plane))
transformation_matrix, inverse_transform = calculate_homogeneous_transforms(local_origin,
local_x_dir,
local_xy_plane)
tm_str = ' {}'.format(transformation_matrix)
tm_str = tm_str.replace('\n', '\n ')
print('\n transformation_matrix = \n{}'.format(tm_str))
point = np.asarray([1.0, 1.0, 2, 1.0])
point_local = inverse_transform.dot(point)
print('\n point {} in local cs = {}'.format(point, point_local))
point_global = transformation_matrix.dot(point_local)
print(' local point {} in global cs = {}\n'.format(point_local, point_global))
assert np.isclose(point, point_global).all()
assert np.isclose(point_local, [1.0, -1, 2, 1.]).all()
print(' Successfully completed test of test_pure_rotation\n')
return None
def test_pure_translation():
print('{}'.format('-'*80))
print('testing test_pure_translation')
local_origin = np.asarray([0.0, 0.0, 1.0])
local_x_dir = np.asarray([1.0, 0, 1.0])
local_xy_plane = np.asarray([0.0, 1.0, 1.0])
print(' local_origin = {}'.format(local_origin))
print(' local_x_dir = {}'.format(local_x_dir))
print(' local_xy_plane = {}'.format(local_xy_plane))
transformation_matrix, inverse_transform = calculate_homogeneous_transforms(local_origin,
local_x_dir,
local_xy_plane)
tm_str = ' {}'.format(transformation_matrix)
tm_str = tm_str.replace('\n', '\n ')
print('\n transformation_matrix = \n{}'.format(tm_str))
point = np.asarray([1.0, 1.0, 0, 1.0])
point_local = inverse_transform.dot(point)
print('\n point {} in local cs = {}'.format(point, point_local))
point_global = transformation_matrix.dot(point_local)
print(' local point {} in global cs = {}\n'.format(point_local, point_global))
assert np.isclose(point, point_global).all()
assert np.isclose(point_local, [1.0, 1, -1, 1.]).all()
print(' Successfully completed test of test_pure_translation\n')
return None
def test_rotation_and_translation():
print('{}'.format('-'*80))
print('testing test_rotation_and_translation')
local_origin = np.asarray([1.0, 1.0, 1.0])
local_x_dir = np.asarray([.0, 1, 1.0])
local_xy_plane = np.asarray([1.0, 2.0, 1.0])
print(' local_origin = {}'.format(local_origin))
print(' local_x_dir = {}'.format(local_x_dir))
print(' local_xy_plane = {}'.format(local_xy_plane))
transformation_matrix, inverse_transform = calculate_homogeneous_transforms(local_origin,
local_x_dir,
local_xy_plane)
tm_str = ' {}'.format(transformation_matrix)
tm_str = tm_str.replace('\n', '\n ')
print('\n transformation_matrix = \n{}'.format(tm_str))
point = np.asarray([-1.0, 2.0, 2, 1.0])
# point = np.asarray([1.0, 1.0, 1, 1.0])
# point = np.asarray([0.0, 0.0, 1, 1.0])
point_local = inverse_transform.dot(point)
print('\n point {} in local cs = {}'.format(point, point_local))
point_global = transformation_matrix.dot(point_local)
print(' local point {} in global cs = {}\n'.format(point_local, point_global))
assert np.isclose(point, point_global).all()
assert np.isclose(point_local, [2.0, 1, -1, 1.]).all()
print(' Successfully completed test of test_rotation_and_translation\n')
return None
if __name__ == '__main__':
test_pure_rotation()
test_pure_translation()
test_rotation_and_translation()
print('')
现在，测试用例产生了非常简单的输出，并展示了如何将单个点从一个坐标系转换到另一个坐标系。
--------------------------------------------------------------------------------
testing test_pure_rotation
local_origin = [0. 0. 0.]
local_x_dir = [0. 1. 0.]
local_xy_plane = [-1. 1. 0.]
transformation_matrix =
[[ 0. -1. 0. 0.]
[ 1. 0. 0. 0.]
[ 0. 0. 1. 0.]
[ 0. 0. 0. 1.]]
point [1. 1. 2. 1.] in local cs = [ 1. -1. 2. 1.]
local point [ 1. -1. 2. 1.] in global cs = [1. 1. 2. 1.]
Successfully completed test of test_pure_rotation
--------------------------------------------------------------------------------
testing test_pure_translation
local_origin = [0. 0. 1.]
local_x_dir = [1. 0. 1.]
local_xy_plane = [0. 1. 1.]
transformation_matrix =
[[1. 0. 0. 0.]
[0. 1. 0. 0.]
[0. 0. 1. 1.]
[0. 0. 0. 1.]]
point [1. 1. 0. 1.] in local cs = [ 1. 1. -1. 1.]
local point [ 1. 1. -1. 1.] in global cs = [1. 1. 0. 1.]
Successfully completed test of test_pure_translation
--------------------------------------------------------------------------------
testing test_rotation_and_translation
local_origin = [1. 1. 1.]
local_x_dir = [0. 1. 1.]
local_xy_plane = [1. 2. 1.]
transformation_matrix =
[[-1. 0. 0. 1.]
[ 0. 1. 0. 1.]
[ 0. 0. -1. 1.]
[ 0. 0. 0. 1.]]
point [-1. 2. 2. 1.] in local cs = [ 2. 1. -1. 1.]
local point [ 2. 1. -1. 1.] in global cs = [-1. 2. 2. 1.]
Successfully completed test of test_rotation_and_translation
要一次使用多个点的代码，我们可以调用以下代码“example_transform_cs.py”。
import numpy as np
import matplotlib.pyplot as plt
import transform_coord_sys as tcs
if __name__ == '__main__':
local_origin = np.asarray([1.0, 1.0, 1.0])
local_x_dir = np.asarray([0, 1, 1.0])
# local_x_dir = np.asarray([-7, 2, -3])
local_xy_plane = np.asarray([1.0, 2.0, 1.0])
print(' local_origin = {}'.format(local_origin))
print(' local_x_dir = {}'.format(local_x_dir))
print(' local_xy_plane = {}'.format(local_xy_plane))
transformation_matrix, inverse_transform = tcs.calculate_homogeneous_transforms(local_origin,
local_x_dir,
local_xy_plane)
tm_str = ' {}'.format(transformation_matrix)
tm_str = tm_str.replace('\n', '\n ')
print('\n transformation_matrix = \n{}'.format(tm_str))
# all points have an extra dimension with the fourth item set to 1.0
# this is to make them compatible with homogeneous transforms
point = np.asarray([.4, 0.6, 0.7, 1.0])
point_local = inverse_transform.dot(point)
print('\n point {} in local cs = {}'.format(point, point_local))
point_global = transformation_matrix.dot(point_local)
print(' local point {} in global cs = {}\n'.format(point_local, point_global))
global_cs_line_x = np.asarray([0.0, 1.0, 0.0, 0.0, 0.0])
global_cs_line_y = np.asarray([0.0, 0.0, 1.0, 0.0, 0.0])
global_cs_line_z = np.asarray([0.0, 0.0, 0.0, 0.0, 1.0])
global_homogenous = np.ones(len(global_cs_line_x))
global_cs_3d = np.concatenate((global_cs_line_x, global_cs_line_y,
global_cs_line_z, global_homogenous)).reshape((4, 5))
local_cs_3d = transformation_matrix.dot(global_cs_3d)
ax = plt.figure().add_subplot(projection='3d')
ax.plot(global_cs_line_x, global_cs_line_y, global_cs_line_z, label='global_cs')
ax.plot(local_cs_3d[0, :], local_cs_3d[1, :], local_cs_3d[2, :], label='local_cs')
ax.plot(point[0], point[1], point[2], label='point', marker='x', markersize=14)
fig = plt.gcf()
text = 'Point\nGlobal CS {}\nLocal CS {}'.format(point[0:3], point_local[0:3])
fig.text(.1, .9, text, fontsize=8)
# ax.set_box_aspect((np.ptp(xs), np.ptp(ys), np.ptp(zs))) # aspect ratio is 1:1:1 in data space
ax.set_box_aspect((1., 1., 1.)) # aspect ratio is 1:1:1 in data space
ax.legend()
plt.show()
该代码创建表示全局坐标系的单位矢量的简单线表示，然后将数据转换为局部坐标系。单个点在全局和局部坐标系中都表示。当然，一张图片确实可以帮助我了解正在发生的事情以及它是否正常工作。
祝你有美好的一天，并付出代价。
于 2021-09-20T15:58:15.587 回答