add tools
This commit is contained in:
liangyuxuan
270
tools/calibration_tools.py
Normal file
270
tools/calibration_tools.py
Normal file
@@ -0,0 +1,270 @@
|
||||
import math
|
||||
import json
|
||||
import logging
|
||||
import argparse
|
||||
|
||||
import numpy as np
|
||||
import transforms3d as tfs
|
||||
|
||||
|
||||
def _get_matrix_quat(x, y, z, rw, rx, ry, rz):
|
||||
"""从单位四元数构建齐次变换矩阵"""
|
||||
'''构造旋转矩阵'''
|
||||
q = [rw, rx, ry, rz]
|
||||
rmat = tfs.quaternions.quat2mat(q)
|
||||
|
||||
"""构造齐次变换矩阵"""
|
||||
rmat = tfs.affines.compose(
|
||||
np.squeeze(np.asarray((x, y, z))),
|
||||
rmat,
|
||||
[1, 1, 1]
|
||||
)
|
||||
return rmat
|
||||
|
||||
|
||||
def _get_matrix_eular_radu(x, y, z, rx, ry, rz):
|
||||
"""从欧拉角构建齐次变换矩阵"""
|
||||
'''构造旋转矩阵'''
|
||||
rmat = tfs.euler.euler2mat(
|
||||
# math.radians(rx), math.radians(ry), math.radians(rz)
|
||||
rx, ry, rz
|
||||
)
|
||||
|
||||
"""构造齐次变换矩阵"""
|
||||
rmat = tfs.affines.compose(
|
||||
np.squeeze(np.asarray((x, y, z))),
|
||||
rmat,
|
||||
[1, 1, 1]
|
||||
)
|
||||
return rmat
|
||||
|
||||
|
||||
def _get_matrix_rotvector(x, y, z, rx, ry, rz):
|
||||
"""从旋转向量构建齐次变换矩阵"""
|
||||
'''构造旋转矩阵'''
|
||||
rvec = np.array([rx, ry, rz])
|
||||
theta = np.linalg.norm(rvec)
|
||||
if theta < 1e-8:
|
||||
rmat = np.eye(3) # 小角度直接返回单位矩阵
|
||||
else:
|
||||
axis = rvec / theta
|
||||
rmat = tfs.axangles.axangle2mat(axis, theta)
|
||||
|
||||
"""构造齐次变换矩阵"""
|
||||
rmat = tfs.affines.compose(
|
||||
np.squeeze(np.asarray((x, y, z))),
|
||||
rmat,
|
||||
[1, 1, 1]
|
||||
)
|
||||
return rmat
|
||||
|
||||
|
||||
def _skew(v):
|
||||
return np.array([[0, -v[2], v[1]],
|
||||
[v[2], 0, -v[0]],
|
||||
[-v[1], v[0], 0]])
|
||||
|
||||
|
||||
def _R2P(T):
|
||||
"""旋转矩阵 --> 修正罗德里格斯向量"""
|
||||
axis, angle= tfs.axangles.mat2axangle(T[0:3, 0:3])
|
||||
P = 2 * math.sin(angle / 2) * axis
|
||||
return P
|
||||
|
||||
|
||||
def _P2R(P):
|
||||
"""修正罗德里格斯向量 --> 旋转矩阵"""
|
||||
P2 = np.dot(P.T, P)
|
||||
part_1 = (1 - P2 / 2) * np.eye(3)
|
||||
part_2 = np.add(np.dot(P, P.T), np.sqrt(4- P2) * _skew(P.flatten().T))
|
||||
R = np.add(part_1, np.divide(part_2, 2))
|
||||
return R
|
||||
|
||||
|
||||
def _calculate_(Hgs, Hcs):
|
||||
"""计算标定矩阵"""
|
||||
# H代表矩阵、h代表标量
|
||||
Hgijs = []
|
||||
Hcijs = []
|
||||
A = []
|
||||
B = []
|
||||
size = 0
|
||||
for i in range(len(Hgs)):
|
||||
for j in range(i + 1, len(Hgs)):
|
||||
size += 1
|
||||
Hgij = np.dot(np.linalg.inv(Hgs[j]), Hgs[i])
|
||||
Hgijs.append(Hgij)
|
||||
Pgij = np.dot(2, _R2P(Hgij))
|
||||
|
||||
Hcij = np.dot(Hcs[j], np.linalg.inv(Hcs[i]))
|
||||
Hcijs.append(Hcij)
|
||||
Pcij = np.dot(2, _R2P(Hcij))
|
||||
|
||||
A.append(_skew(np.add(Pgij, Pcij)))
|
||||
B.append(np.subtract(Pcij, Pgij).reshape(3, 1))
|
||||
|
||||
MA = np.vstack(A)
|
||||
MB = np.vstack(B)
|
||||
Pcg_ = np.dot(np.linalg.pinv(MA), MB)
|
||||
pcg = np.sqrt(np.add(1, np.dot(Pcg_.T, Pcg_)))
|
||||
Pcg = np.dot(np.dot(2, Pcg_), np.linalg.inv(pcg))
|
||||
Rcg = _P2R(Pcg).reshape(3, 3)
|
||||
|
||||
A = []
|
||||
B = []
|
||||
id = 0
|
||||
for i in range(len(Hgs)):
|
||||
for j in range(i + 1, len(Hgs)):
|
||||
Hgij = Hgijs[id]
|
||||
Hcij = Hcijs[id]
|
||||
A.append(np.subtract(Hgij[0:3, 0:3], np.eye(3, 3)))
|
||||
B.append(np.subtract(np.dot(Rcg, Hcij[0:3, 3:4]), Hgij[0:3, 3:4]))
|
||||
id += 1
|
||||
|
||||
MA = np.asarray(A).reshape(size * 3, 3)
|
||||
MB = np.asarray(B).reshape(size * 3, 1)
|
||||
Tcg = np.dot(np.linalg.pinv(MA), MB).reshape(3, )
|
||||
|
||||
return tfs.affines.compose(Tcg, np.squeeze(Rcg), [1, 1, 1])
|
||||
|
||||
|
||||
class _Calibration:
|
||||
def __init__(self, mode, input_format, hand_pose_path, camera_pose_path, save_path):
|
||||
"""
|
||||
matrix_name: str
|
||||
mode: "eye_in_hand" or "eye_to_hand"
|
||||
input_format: str, "euler" or "rotvertor" or "quat"
|
||||
hand_pose_path: str
|
||||
camera_pose_path: str
|
||||
save_path: str
|
||||
"""
|
||||
self._Hgs, self._Hcs = [], []
|
||||
self._hand, self._camera = [], []
|
||||
self._calibration_matrix = None
|
||||
self._done = False
|
||||
|
||||
self._mode = mode
|
||||
if self._mode not in ['eye_in_hand', 'eye_to_hand']:
|
||||
raise ValueError("mode must be 'eye_in_hand' or 'eye_to_hand'")
|
||||
|
||||
self._input = input_format
|
||||
if self._input == 'euler':
|
||||
self._function = _get_matrix_eular_radu
|
||||
elif self._input == 'rotvertor':
|
||||
self._function = _get_matrix_rotvector
|
||||
elif self._input == 'quat':
|
||||
self._function = _get_matrix_quat
|
||||
else:
|
||||
raise ValueError("INPUT ERROR")
|
||||
|
||||
self._camera_pose_path = camera_pose_path
|
||||
self._hand_pose_path = hand_pose_path
|
||||
self._save_path = save_path
|
||||
|
||||
self._get_pose()
|
||||
|
||||
def _get_pose(self):
|
||||
with open(f'{self._camera_pose_path}', 'r', encoding='utf-8') as f:
|
||||
self._camera = json.load(f)
|
||||
|
||||
with open(f'{self._hand_pose_path}', 'r', encoding='utf-8') as f:
|
||||
self._hand = json.load(f)
|
||||
|
||||
self._calculate_calibration()
|
||||
|
||||
print(self._hand)
|
||||
print(self._camera)
|
||||
|
||||
logging.info(f"{self._calibration_matrix}")
|
||||
hand_eye_result = {
|
||||
"T": self._calibration_matrix.tolist()
|
||||
}
|
||||
with open(f"{self._save_path}", "w") as f:
|
||||
json.dump(hand_eye_result, f, indent=4)
|
||||
logging.info(f"Save as {self._save_path}")
|
||||
|
||||
self._done = True
|
||||
|
||||
def _calculate_data(self):
|
||||
if self._input == 'quat':
|
||||
for i in range(0, len(self._hand), 7):
|
||||
self._Hgs.append(
|
||||
np.linalg.inv(
|
||||
self._function(
|
||||
self._hand[i], self._hand[i + 1], self._hand[i + 2],
|
||||
self._hand[i + 3], self._hand[i + 4], self._hand[i + 5], self._hand[i + 6]
|
||||
)
|
||||
)
|
||||
if self._mode == 'eye_to_hand' else
|
||||
self._function(
|
||||
self._hand[i], self._hand[i + 1], self._hand[i + 2],
|
||||
self._hand[i + 3], self._hand[i + 4], self._hand[i + 5], self._hand[i + 6]
|
||||
)
|
||||
)
|
||||
self._Hcs.append(
|
||||
self._function(
|
||||
self._camera[i], self._camera[i + 1], self._camera[i + 2],
|
||||
self._camera[i + 3], self._camera[i + 4], self._camera[i + 5], self._camera[i + 6]
|
||||
)
|
||||
)
|
||||
|
||||
else:
|
||||
for i in range(0, len(self._hand), 6):
|
||||
self._Hgs.append(
|
||||
np.linalg.inv(
|
||||
self._function(
|
||||
self._hand[i], self._hand[i + 1], self._hand[i + 2],
|
||||
self._hand[i + 3], self._hand[i + 4], self._hand[i + 5]
|
||||
)
|
||||
)
|
||||
if self._mode == 'eye_to_hand' else
|
||||
self._function(
|
||||
self._hand[i], self._hand[i + 1], self._hand[i + 2],
|
||||
self._hand[i + 3], self._hand[i + 4], self._hand[i + 5]
|
||||
)
|
||||
)
|
||||
self._Hcs.append(
|
||||
self._function(
|
||||
self._camera[i], self._camera[i + 1], self._camera[i + 2],
|
||||
self._camera[i + 3], self._camera[i + 4], self._camera[i + 5]
|
||||
)
|
||||
)
|
||||
|
||||
def _calculate_calibration(self):
|
||||
self._calculate_data()
|
||||
self._calibration_matrix = _calculate_(self._Hgs, self._Hcs)
|
||||
|
||||
|
||||
def calculate_hand_eye_matrix(
|
||||
mode,
|
||||
input_format,
|
||||
hand_pose_path,
|
||||
camera_pose_path,
|
||||
save_path
|
||||
):
|
||||
"""
|
||||
matrix_name: str
|
||||
mode: "eye_in_hand" or "eye_to_hand"
|
||||
input_format: str, "euler" or "rotvertor" or "quat"
|
||||
hand_pose_path: str
|
||||
camera_pose_path: str
|
||||
save_path: str
|
||||
"""
|
||||
a = _Calibration(mode, input_format, hand_pose_path, camera_pose_path, save_path)
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--mode", type=str, required=True)
|
||||
parser.add_argument("--hand_path", type=str, required=True)
|
||||
parser.add_argument("--camera_path", type=str, required=True)
|
||||
parser.add_argument("--save_path", type=str, required=True)
|
||||
parser.add_argument("--input_format", type=str, default="quat")
|
||||
args = parser.parse_args()
|
||||
|
||||
calculate_hand_eye_matrix(
|
||||
mode= args.mode,
|
||||
input_format= args.input_format,
|
||||
hand_pose_path=args.hand_path,
|
||||
camera_pose_path=args.camera_path,
|
||||
save_path=args.save_path
|
||||
)
|
||||
54
tools/data_tools.py
Normal file
54
tools/data_tools.py
Normal file
@@ -0,0 +1,54 @@
|
||||
import argparse
|
||||
import json
|
||||
import transforms3d as tfs
|
||||
|
||||
def prepare_data(_data:list[float], save_path: str = "camera_pose_data.json", format: str = "quat_xyzw", mm2m: bool = False):
|
||||
new_list = []
|
||||
if format == "quat_wxyz":
|
||||
for i in range(0, len(_data), 7):
|
||||
if not mm2m:
|
||||
new = [_data[i], _data[i + 1], _data[i + 2], _data[i + 3], _data[i + 4], _data[i + 5], _data[i + 6]]
|
||||
else:
|
||||
new = [_data[i]/1e3, _data[i + 1]/1e3, _data[i + 2]/1e3, _data[i + 6], _data[i + 3], _data[i + 4], _data[i + 5], _data[i + 6]]
|
||||
new_list.extend(new)
|
||||
elif format == "quat_xyzw":
|
||||
for i in range(0, len(_data), 7):
|
||||
if not mm2m:
|
||||
new = [_data[i], _data[i + 1], _data[i + 2], _data[i + 6], _data[i + 3], _data[i + 4], _data[i + 5]]
|
||||
else:
|
||||
new = [_data[i] / 1e3, _data[i + 1] / 1e3, _data[i + 2] / 1e3, _data[i + 6], _data[i + 6], _data[i + 3], _data[i + 4], _data[i + 5]]
|
||||
new_list.extend(new)
|
||||
|
||||
elif format == "euler":
|
||||
for i in range(0, len(_data), 6):
|
||||
rw, rx, ry, rz = tfs.euler.euler2quat(_data[i + 3], _data[i + 4], _data[i + 5], axes='sxyz')
|
||||
if not mm2m:
|
||||
new = [_data[i], _data[i + 1], _data[i + 2], rw, rx, ry, rz]
|
||||
else:
|
||||
new = [_data[i]/1e3, _data[i + 1]/1e3, _data[i + 2]/1e3, rw, rx, ry, rz]
|
||||
new_list.extend(new)
|
||||
|
||||
elif format == "rotvertor":
|
||||
for i in range(0, len(_data), 6):
|
||||
# tfs.quaternions.rotvec2quat
|
||||
if not mm2m:
|
||||
new = [_data[i], _data[i + 1], _data[i + 2], _data[i + 3], _data[i + 4], _data[i + 5]]
|
||||
else:
|
||||
new = [_data[i]/1e3, _data[i + 1]/1e3, _data[i + 2]/1e3, _data[i + 6], _data[i + 3], _data[i + 4], _data[i + 5]]
|
||||
new_list.extend(new)
|
||||
|
||||
else: pass
|
||||
with open(save_path, 'w') as f: json.dump(new_list, f, indent=4)
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--data_path", type=str, required=True)
|
||||
parser.add_argument("--save_path", type=str, required=True)
|
||||
parser.add_argument("--format", type=str, default="quat_xyzw")
|
||||
parser.add_argument("--mm2m", type=bool, default=False)
|
||||
args = parser.parse_args()
|
||||
|
||||
with open(args.data_path, "r") as f:
|
||||
_data = json.load(f)
|
||||
|
||||
prepare_data(_data, args.save_path, args.format, args.mm2m)
|
||||
89
tools/error_calculation_tool.py
Normal file
89
tools/error_calculation_tool.py
Normal file
@@ -0,0 +1,89 @@
|
||||
import argparse
|
||||
|
||||
import numpy as np
|
||||
import transforms3d as tfs
|
||||
import json
|
||||
|
||||
|
||||
def get_matrix_euler_radu(x, y, z, rx, ry, rz):
|
||||
"""从欧拉角构建齐次变换矩阵"""
|
||||
'''构造旋转矩阵'''
|
||||
rmat = tfs.euler.euler2mat(
|
||||
# math.radians(rx), math.radians(ry), math.radians(rz)
|
||||
rx, ry, rz
|
||||
)
|
||||
|
||||
"""构造齐次变换矩阵"""
|
||||
rmat = tfs.affines.compose(
|
||||
np.squeeze(np.asarray((x, y, z))),
|
||||
rmat,
|
||||
[1, 1, 1]
|
||||
)
|
||||
return rmat
|
||||
|
||||
|
||||
def get_matrix_quat(x, y, z, rw, rx, ry, rz):
|
||||
"""从单位四元数构建齐次变换矩阵"""
|
||||
'''构造旋转矩阵'''
|
||||
q = [rw, rx, ry, rz]
|
||||
rmat = tfs.quaternions.quat2mat(q)
|
||||
|
||||
"""构造齐次变换矩阵"""
|
||||
rmat = tfs.affines.compose(
|
||||
np.squeeze(np.asarray((x, y, z))),
|
||||
rmat,
|
||||
[1, 1, 1]
|
||||
)
|
||||
return rmat
|
||||
|
||||
def calculate(mode, hand_path, camera_path, hand_eye_path):
|
||||
with open(camera_path, 'r', encoding='utf-8') as f:
|
||||
camera_all = json.load(f)
|
||||
|
||||
with open(hand_path, 'r', encoding='utf-8') as f:
|
||||
hand_all = json.load(f)
|
||||
|
||||
with open(hand_eye_path, 'r', encoding='utf-8') as f:
|
||||
data = json.load(f)
|
||||
T_cam_in_hand = data["T"]
|
||||
|
||||
sum_ = int(len(hand_all) / 7)
|
||||
T_list = []
|
||||
for i in range(sum_):
|
||||
hand = [hand_all[i*7], hand_all[i*7+1], hand_all[i*7+2], hand_all[i*7+3], hand_all[i*7+4], hand_all[i*7+5], hand_all[i*7+6]]
|
||||
camera = [camera_all[i*7], camera_all[i*7+1], camera_all[i*7+2], camera_all[i*7+3], camera_all[i*7+4], camera_all[i*7+5], camera_all[i*7+6]]
|
||||
T_hand_in_base = get_matrix_quat(hand[0], hand[1], hand[2], hand[3], hand[4], hand[5], hand[6])
|
||||
T_cal_in_camera = get_matrix_quat(camera[0], camera[1], camera[2], camera[3], camera[4], camera[5], camera[6])
|
||||
|
||||
if mode == "eye_to_hand":
|
||||
T = np.linalg.inv(T_hand_in_base) @ T_cam_in_hand @ T_cal_in_camera
|
||||
else:
|
||||
T = T_hand_in_base @ T_cam_in_hand @ T_cal_in_camera
|
||||
T_list.append(T)
|
||||
print(T)
|
||||
|
||||
sum_sub = np.zeros_like(T_list[0])
|
||||
for i in range(len(T_list)):
|
||||
for j in range(i + 1, len(T_list)):
|
||||
d = np.subtract(T_list[i], T_list[j])
|
||||
sum_sub = np.add(sum_sub, d)
|
||||
|
||||
len_sub = len(T_list) * (len(T_list) - 1) / 2
|
||||
|
||||
p = np.divide(sum_sub, len_sub)
|
||||
p[0:3 ,3:4] = p[0:3 ,3:4] * 1000
|
||||
np.set_printoptions(suppress=True, floatmode='fixed', precision=8)
|
||||
print(p)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("mode", type=str)
|
||||
parser.add_argument("hand_path", type=str)
|
||||
parser.add_argument("camera_path", type=str)
|
||||
parser.add_argument("hand_eye_path", type=str)
|
||||
args = parser.parse_args()
|
||||
|
||||
calculate(args.mode, args.hand_path, args.camera_path, args.hand_eye_path)
|
||||
|
||||
|
||||
23
tools/pt2engiene.py
Normal file
23
tools/pt2engiene.py
Normal file
@@ -0,0 +1,23 @@
|
||||
import argparse
|
||||
|
||||
from ultralytics import YOLO
|
||||
|
||||
|
||||
def main(checkpoint_path):
|
||||
model = YOLO(checkpoint_path)
|
||||
model.export(
|
||||
format="engine",
|
||||
imgsz=(640, 480),
|
||||
dynamic=True,
|
||||
simplify=True,
|
||||
half=True,
|
||||
workspace=0.8,
|
||||
batch=1,
|
||||
device=0
|
||||
)
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("checkpoint_path", type=str)
|
||||
args = parser.parse_args()
|
||||
main(args.checkpoint_path)
|
||||
50
tools/read_stl.py
Normal file
50
tools/read_stl.py
Normal file
@@ -0,0 +1,50 @@
|
||||
import logging
|
||||
|
||||
import numpy as np
|
||||
import open3d as o3d
|
||||
|
||||
|
||||
def read_stl(stl_path: str, save_path: str, view_pointcloud: bool = False) -> None:
|
||||
mesh = o3d.io.read_triangle_mesh(stl_path)
|
||||
mesh.scale(0.001, center=[0, 0, 0])
|
||||
mesh.compute_vertex_normals()
|
||||
|
||||
pcd_model = mesh.sample_points_uniformly(number_of_points=50000)
|
||||
# pcd_model = pcd_model.voxel_down_sample(voxel_size=0.01)
|
||||
|
||||
center = pcd_model.get_center().flatten()
|
||||
R = np.eye(4)
|
||||
R[0, 3], R[1, 3], R[2, 3] = -center
|
||||
pcd_model.transform(R)
|
||||
|
||||
R = np.array(
|
||||
[[0, 0, 1, 0],
|
||||
[0, 1, 0, 0],
|
||||
[-1, 0, 0, 0],
|
||||
[0, 0, 0, 1]],
|
||||
)
|
||||
|
||||
pcd_model.transform(R)
|
||||
|
||||
if view_pointcloud:
|
||||
point = [
|
||||
[0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]
|
||||
] # 画点：原点、第一主成分、第二主成分
|
||||
lines = [
|
||||
[0, 1], [0, 2], [0, 3]
|
||||
] # 画出三点之间两两连线
|
||||
colors = [
|
||||
[1, 0, 0], [0, 1, 0], [0, 0, 1]
|
||||
]
|
||||
# 构造open3d中的LineSet对象，用于主成分显示
|
||||
line_set = o3d.geometry.LineSet(points=o3d.utility.Vector3dVector(point), lines=o3d.utility.Vector2iVector(lines))
|
||||
line_set.colors = o3d.utility.Vector3dVector(colors)
|
||||
|
||||
o3d.visualization.draw_geometries([pcd_model, line_set])
|
||||
|
||||
# success = o3d.io.write_point_cloud(save_path, pcd_model)
|
||||
# logging.info(f"PCD_0, 写入结果: {success}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
read_stl()
|
||||
Reference in New Issue
Block a user