70 lines
2.3 KiB
Python
70 lines
2.3 KiB
Python
# -- coding: utf-8 --
|
|
import inspect
|
|
import logging
|
|
import logging.config
|
|
import numpy as np
|
|
import os
|
|
import json
|
|
import numpy as np
|
|
from scipy.spatial.transform import Rotation as R
|
|
import cv2
|
|
import config
|
|
import utils
|
|
import svd
|
|
from scipy.spatial import KDTree
|
|
# import matplotlib.pyplot as plt
|
|
|
|
if __name__ == "__main__":
|
|
|
|
all_points = utils.read_from_json(os.path.dirname(__file__) + '\\record\\points.json')
|
|
records = utils.read_from_json(os.path.dirname(__file__) + '\\record\\record.json')
|
|
aurco_marks_conners_all = all_points[0]
|
|
pcd_marks_conners_all = all_points[1]
|
|
|
|
# print(aurco_marks_conners_all)
|
|
# print(pcd_marks_conners_all)
|
|
|
|
_R, _t, _, _ = svd.scipy_svd(
|
|
np.asarray(pcd_marks_conners_all),
|
|
np.asarray(aurco_marks_conners_all))
|
|
_r, _p, _y = utils.rotation_matrix_to_rpy(_R)
|
|
# 计算从初始值到最终rt的结果
|
|
_new_rpy = utils.combine_rpy(config.roll, config.pitch, config.yaw, _r, _p, _y)
|
|
# print(_R)
|
|
print(_new_rpy)
|
|
print(_t)
|
|
|
|
# 优化 RT
|
|
# RT_matrices = []
|
|
# for i in range(len(records)):
|
|
# record = records[i]
|
|
# _r, _p, _y = record[4]
|
|
# _t = np.asarray(record[3])
|
|
# _R = utils.rpy_to_rotation_matrix(_r, _p, _y)
|
|
# RT_matrices.append([_R, _t])
|
|
# R_opt, t_opt = utils.filter_and_fit_rts(
|
|
# RT_matrices, np.asarray(pcd_marks_conners_all), np.asarray(aurco_marks_conners_all),
|
|
# threshold=0.001, use_ransac=True)
|
|
# _r, _p, _y = utils.rotation_matrix_to_rpy(R_opt)
|
|
# _new_rpy = utils.combine_rpy(config.roll, config.pitch, config.yaw, _r, _p, _y)
|
|
# print(_new_rpy)
|
|
# print(t_opt)
|
|
|
|
# _pdc_rt = (R_opt @ np.asarray(pcd_marks_conners_all).T).T + t_opt
|
|
|
|
_pdc_rt = (_R @ np.asarray(pcd_marks_conners_all).T).T + _t
|
|
|
|
distances = []
|
|
cnt = 0
|
|
_distances = 0
|
|
for i in range(len(_pdc_rt)):
|
|
# print(np.linalg.norm(_pdc_rt[i] - aurco_marks_conners_all[i]))
|
|
temp_distanece = np.linalg.norm(_pdc_rt[i] - aurco_marks_conners_all[i])
|
|
distances.append(temp_distanece)
|
|
_distances += temp_distanece
|
|
cnt += 1
|
|
|
|
# print(distances)
|
|
print("均值:", _distances/cnt)
|
|
# plt.plot(distances)
|
|
# plt.show() |