空間に2つの3Dオブジェクトがあり、あるオブジェクトから別のオブジェクトにポイントをコピーしたいと思います。問題は、これらのオブジェクトが共通の座標系を共有しておらず、座標変換を行う必要があることです。両方のオブジェクトのローカル変換行列があり、ワールド変換行列にもアクセスできます。これらの変換行列を使用して実行する計算がいくつかあることは知っていますが、その方法がわかりません。
他のオブジェクト（またはその座標系）にコピーした場合、最初のオブジェクトの1つのポイントを（ワールド座標に対して）同じ位置になるように変換するにはどうすればよいですか？
ありがとう
さて、あなたが持っている変換演算子を適用する必要があります。たとえば、極座標 (r, t) とデカルト座標 (x, y) の関係は次のように定義されます。
x = rcost
y = rsint
私は約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('')
これで、テスト ケースの結果は非常に単純な出力になり、1 つの点をある座標系から別の座標系に変換する方法が示されます。
--------------------------------------------------------------------------------
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()
このコードは、グローバル座標系を表す単位ベクトルの単純なライン表現を作成し、データをローカル座標系に変換します。1 つの点は、グローバル座標系とローカル座標系の両方で表されます。もちろん、写真は何が起こっているのか、正しく機能しているかどうかを理解するのに本当に役立ちます。
良い一日を過ごして、前払いしてください。