import open3d as o3d
import numpy as np
import copy
########################### 과제 1번
filepath = "datafile/cropped_pcd.ply"
pcd1 = o3d.io.read_point_cloud(filepath)
pcd2 = copy.deepcopy(pcd1)
center = pcd1.get_center()
# 변환 행렬
T1 = np.eye(4)
T1[:3, 3] = -center
print("변환행렬:\n", T1)
pcd2.transform(T1)
axis_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5)
o3d.visualization.draw_geometries([pcd1, pcd2, axis_frame], width=500, height=500)
########################### 과제 2번
mesh_cylinder1 = o3d.geometry.TriangleMesh.create_cylinder(radius=0.05, height=0.5)
mesh_cylinder1.compute_vertex_normals()
mesh_cylinder1.paint_uniform_color([0.1, 0.1, 0.7])
mesh_cylinder2 = copy.deepcopy(mesh_cylinder1)
mesh_cylinder3 = copy.deepcopy(mesh_cylinder1)
mesh_cylinder2.paint_uniform_color([0.7, 0.1, 0.1])
mesh_cylinder3.paint_uniform_color([0.1, 0.7, 0.1])
R_y = o3d.geometry.get_rotation_matrix_from_xyz((0, np.pi / 2, 0))
R_x = o3d.geometry.get_rotation_matrix_from_xyz((np.pi / 2, 0, 0))
T2 = np.eye(4)
T2[:3, :3] = R_y
print("mesh_cylinder2 회전행렬:\n", T2)
mesh_cylinder2.transform(T2)
T3 = np.eye(4)
T3[:3, :3] = R_x
print("mesh_cylinder3 회전행렬:\n", T3)
mesh_cylinder3.transform(T3)
axis_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5)
o3d.visualization.draw_geometries([mesh_cylinder1, mesh_cylinder2, mesh_cylinder3, axis_frame], width=500, height=500)
########################### 과제 3번
mesh_cylinder1 = o3d.geometry.TriangleMesh.create_cylinder(radius=0.05, height=0.5)
mesh_cylinder1.compute_vertex_normals()
mesh_cylinder1.paint_uniform_color([0.1, 0.1, 0.7])
mesh_cylinder3 = copy.deepcopy(mesh_cylinder1)
mesh_cylinder3.paint_uniform_color([0.1, 0.7, 0.1])
R_x = o3d.geometry.get_rotation_matrix_from_xyz((np.pi / 2, 0, 0))
T = np.eye(4)
T[:3, :3] = R_x
T[0, 3] = 0.3 # x방향 이동
print("회전+이동 행렬:\n", T)
mesh_cylinder3.transform(T)
axis_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5)
o3d.visualization.draw_geometries([mesh_cylinder1, mesh_cylinder3, axis_frame], width=500, height=500)