-
Notifications
You must be signed in to change notification settings - Fork 14
/
common3Dfunc.py
132 lines (105 loc) · 3.4 KB
/
common3Dfunc.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
# Common functions for 3D processiong
# Shuichi Akizuki, Chukyo Univ.
# Email: [email protected]
#
import open3d as o3d
import numpy as np
import copy
import json
from math import *
def Centering( _cloud_in ):
"""
Centering()
offset an input cloud to its centroid.
input(s):
_cloud_in: point cloud to be centered
output(s):
cloud_off:
"""
cloud_in = copy.deepcopy(_cloud_in)
np_m = np.asarray(cloud_in.points)
center = np.mean(np_m, axis=0)
np_m[:] -= center
cloud_off = o3d.geometry.PointCloud()
cloud_off.points = o3d.utility.Vector3dVector(np_m)
return cloud_off, center
def ComputeTransformationMatrixAroundCentroid( _cloud_in, _roll, _pitch, _yaw ):
""" offset center """
np_in = np.asarray(_cloud_in.points)
center = np.mean(np_in, axis=0)
offset = np.identity(4)
offset[0:3,3] -= center
""" rotation """
rot = RPY2Matrix4x4( _roll, _pitch, _yaw )
""" reverse offset """
reverse = np.identity(4)
reverse[0:3,3] = center
final = np.dot( reverse, np.dot( rot, offset ) )
return final
def Scaling( cloud_in, scale ):
"""
multiply scaling factor to the input point cloud.
input(s):
cloud_in: point cloud to be scaled.
scale: scaling factor
output(s):
cloud_out:
"""
cloud_np = np.asarray(cloud_in.points)
cloud_np *= scale
cloud_out = o3d.geometry.PointCloud()
cloud_out.points = o3d.utility.Vector3dVector(cloud_np)
return cloud_out
def Offset( cloud_in, offset ):
cloud_np = np.asarray(cloud_in.points)
cloud_np += offset
cloud_off = o3d.geometry.PointCloud()
cloud_off.points = o3d.utility.Vector3dVector(cloud_np)
return cloud_off
def RPY2Matrix4x4( roll, pitch, yaw ):
rot = np.identity(4)
if roll< -3.141:
roll += 6.282
elif 3.141 < roll:
roll -= 6.282
if pitch < -3.141:
pitch += 6.282
elif 3.141 < pitch:
pitch -= 6.282
if yaw < -3.141:
yaw += 6.282
elif 3.141 < yaw:
yaw -= 6.282
rot[ 0, 0 ] = cos(yaw)*cos(pitch)
rot[ 0, 1 ] = -sin(yaw)*cos(roll) + (cos(yaw)*sin(pitch)*sin(roll))
rot[ 0, 2 ] = sin(yaw)*sin(roll) + (cos(yaw)*sin(pitch)*cos(roll))
rot[ 1, 0 ] = sin(yaw)*cos(pitch)
rot[ 1, 1 ] = cos(yaw)*cos(roll) + (sin(yaw)*sin(pitch)*sin(roll))
rot[ 1, 2 ] = -cos(yaw)*sin(roll) + (sin(yaw)*sin(pitch)*cos(roll))
rot[ 2, 0 ] = -sin(pitch)
rot[ 2, 1 ] = cos(pitch)*sin(roll)
rot[ 2, 2 ] = cos(pitch)*cos(roll)
rot[ 3, 0 ] = rot[ 3, 1 ] = rot[ 3, 2 ] = 0.0
rot[ 3, 3 ] = 1.0
return rot
def Mat2RPY( rot ):
roll = atan2( rot[2, 1], rot[2, 2] )
pitch = atan2(-rot[2, 0], sqrt( rot[2, 1]*rot[2, 1]+rot[2, 2]*rot[2, 2] ) )
yaw = atan2( rot[1, 0], rot[0, 0] )
return roll, pitch, yaw
def makeTranslation4x4( offset ):
trans = np.identity(4)
trans[0:3,3] = offset
return trans
""" save transformation matrix as a .json file. """
def save_transformation( trans, name ):
trans_list = trans.tolist()
transform = {"transformation4x4":[trans_list]}
f_out = open( name, 'w')
json.dump( transform, f_out )
""" load transformation matrix from a .json file. """
def load_transformation( name ):
f_in = open( name, 'r')
json_data = json.load(f_in)
trans_in = np.array(json_data["transformation4x4"][0])
return trans_in