https://github.com/Dongvdong/v1_1_slam_tool
链接：https://pan.baidu.com/s/1ntXJch2t3GcLJhFBXFrBdw?pwd=kles
提取码：kles
注意事项
R的行列式算出来如果是-1 需要给R的第三列取符号，不然会导致两个平面（RTK采集数据厘米级别精度平面飞行， 非三维空间上下而是退化到二维平面计算srt）计算的t是反着的
import random
import math
import numpy as np
import os
def API_pose_estimation_3dTo3d_ransac(points_src, points_dst): #NED -> slam
p = np.array(points_src, dtype=float)
q = np.array(points_dst, dtype=float)
print("匹配点数: ", len(points_src), " ", len(points_dst))
# 1.计算s并去质心
mean_p = np.mean(p, axis=0)
mean_q = np.mean(q, axis=0)
p_norm = p - mean_p
q_norm = q - mean_q
# 计算距离比
iter_num = 0
_s = 0
inliner_num = 0
ransac_time=2000 # ransac随机抽取验证次数
while iter_num < ransac_time:
# 随机挑选两个元素
_list = []
# print("len(points_src): ",len(points_src))
# if len(points_src) < 2:
# break
inx_1, inx_2 = random.sample(range(len(points_src)), 2)
#print("inx_1: ",inx_1,"inx_2: ",inx_2)
# print("inx_2: ",inx_2)
p_r = np.array([points_src[inx_1], points_src[inx_2]], dtype=float)
q_r = np.array([points_dst[inx_1], points_dst[inx_2]], dtype=float)
_list.append(inx_1)
_list.append(inx_2)
# 计算s
p_norm_r = p_r - mean_p
q_norm_r = q_r - mean_q
# 所有点的xyz平方求和
d1_list = []
d2_list = []
for i in range(len(q_norm_r)):
d1 = q_norm_r[i]
d2 = p_norm_r[i]
dist1 = math.sqrt(np.sum(d1**2))
dist2 = math.sqrt(np.sum(d2**2))
d1_list.append(dist1)
d2_list.append(dist2)
s_r = np.sum(d1_list) / np.sum(d2_list)
# 计算其他点s的误差值
inliner_p = [points_src[inx_1], points_src[inx_2]]
inliner_q = [points_dst[inx_1], points_dst[inx_2]]
for inx in range(len(points_src)):
# 计算点不参与验证
if inx == inx_1 or inx == inx_2:
continue
p_src = np.array(points_src[inx])
q_dst = np.array(points_dst[inx])
# 分别计算到质心距离
p_src_norm = p_src - mean_p
q_dst_norm = q_dst - mean_q
p_src_norm_dist = math.sqrt(np.sum(p_src_norm**2))
q_dst_norm_dist = math.sqrt(np.sum(q_dst_norm**2))
# 计算误差
cal_dist = p_src_norm_dist * s_r
error = cal_dist - q_dst_norm_dist
if abs(error) < 3:
inliner_p.append(points_src[inx])
inliner_q.append(points_dst[inx])
_list.append(inx)
# 利用所有内点计算新的s
p_r = np.array(inliner_p)
q_r = np.array(inliner_q)
p_norm_f = p_r - mean_p
q_norm_f = q_r - mean_q
d1_list = []
d2_list = []
for i in range(len(q_norm_f)):
d1 = q_norm_f[i]
d2 = p_norm_f[i]
dist1 = math.sqrt(np.sum(d1**2))
dist2 = math.sqrt(np.sum(d2**2))
d1_list.append(dist1)
d2_list.append(dist2)
s_final = np.sum(d1_list) / np.sum(d2_list)
# 记录内点数最高的模型
if inliner_num < len(inliner_p):
_s = s_final
inliner_num = len(inliner_p)
inx_list = _list
iter_num += 1
s = _s
# 2.用s缩放src到dst尺度下
p = s * p
mean_p = np.mean(p, axis=0)
p_norm = p - mean_p
# 2.计算q1*q2^T(注意顺序：q2->q1，x是dst,y是src)
N = len(p)
W = np.zeros((3, 3))
for i in range(N):
x = q_norm[i, :] # 每一行数据
x = x.reshape(3, 1) # 3行1列格式 一维数组借助reshape转置
y = p_norm[i, :]
y = y.reshape(1, 3)
W += np.matmul(x, y)
# 3.SVD分解W
# python 线性代数库中svd求出的V与C++ Eigen库中求的V是转置关系
U, sigma, VT = np.linalg.svd(W, full_matrices=True)
# 旋转矩阵R
R = np.matmul(U, VT) # 这里无需再对V转置
# 在寻找旋转矩阵时，有一种特殊情况需要注意。有时SVD会返回一个“反射”矩阵，这在数值上是正确的，但在现实生活中实际上是无意义的。
# 通过检查R的行列式（来自上面的SVD）并查看它是否为负（-1）来解决。如果是，则V的第三列乘以-1。
# 验证R行列式是否为负数 参考链接:https://blog.csdn.net/sinat_29886521/article/details/77506426
# R为-1 会导致两个平面计算方向相反 导致T方向相反 所以要吧R取符号
# 方案1 R=SD的逆=UV的逆 V第三列取负号
# 方案2 按照SLam14讲解R的第三行给负1
if np.linalg.det(R) < 0:
R_temp=np.matmul(U, VT) # R=SD=UV的转置
R_temp_det = np.linalg.det(R_temp) #其实就是-1
# VT 第三列给负号
VT_3col_to_fu1 = np.array([[1, 0, 0], [0, 1, 0], [0, 0, R_temp_det]])
VT_new= np.matmul(VT, VT_3col_to_fu1)
# 重新计算 R 等同于R的第三列直接取负号
R = np.matmul(U, VT_new)
# 平移向量
T = mean_q - np.matmul(R, mean_p) # dst - src
T = T.reshape(3, 1)
sR = s*R
RT_34 = np.c_[sR, T]
# 4.计算误差值
p = np.array(points_src)
error_sum = 0
inx_list2 = []
error_ENU = []
for i in range(N):
src = p[i, :]
dst = q[i, :]
src = src.reshape(3, 1)
dst = dst.reshape(3, 1)
test_dst = np.matmul(sR, src) + T
error_Mat = test_dst - dst
error_Mat2 = error_Mat**2
error = math.sqrt(np.sum(error_Mat2))
error_ENU.append(error)
if error < 3:
inx_list2.append(i)
error_sum += error
print("mean error:", error_sum/N)
print("max error:", max(error_ENU))
print("RT_34:\n", RT_34)
print("缩放系数s:\n", s)
print("旋转矩阵R:\n", R)
print("通尺度下的T:\n", T)
return RT_34, sR, T
# 根据 srt 将单个目标点云变换到指定坐标系下
def API_src3D_sRt_dis3D_one(points_src,SR,T):
points_src_ = [[points_src[0]], [points_src[1]], [points_src[2]]]
points_dis_ = np.matmul(SR,points_src_) + T
#points_dis_ = SR @ points_src_ + T
points_dis_t=[points_dis_[0][0],points_dis_[1][0],points_dis_[2][0]]
return points_dis_t
# 将1组3d点 根据 srt变换到另一个坐标系下
def API_src3D_sRt_dis3D_list(points_src,points_dst,SR,T):
points_dis_t_list=[]
error_sum=0 # 误差计算
for p_i in range(0,len(points_src)):
# 根据srt计算便函后的x y z平移点
points_dis_t=API_src3D_sRt_dis3D_one(points_src[p_i],SR,T)
print("原始点",points_src[p_i],"变换后的点",points_dis_t,"真值",points_dst[p_i])
points_dis_t_list.append(points_dis_t)
# =========整体转换后的计算误差===============
points_dis_t = np.array(points_dis_t)
points_dst[p_i] = np.array(points_dst[p_i])
error_Mat = points_dis_t - points_dst[p_i]
error_Mat2 = error_Mat**2
error = np.sum(error_Mat2)
error_sum += error
error_sum=math.sqrt(error_sum)/len(points_src)
print("平均误差:",error_sum)
return points_dis_t_list
'''
if __name__ == "__main__":
# points_src=[[1,1,1],[2,2,2],[3,3,3]]
# points_dst=[[11,11,11],[21,21,21],[31,31,31]]
# RT_34, SR, T = API_pose_estimation_3dTo3d_ransac(points_src, points_dst) #
# points_dis_t_list=API_src3D_sRt_dis3D_list(points_src,points_dst,SR, T)
# print("变换后的列表",points_dis_t_list)
'''
'''
# # 4 数据转化 为3D-3D计算相似变换准备 colmap enu 变换到 gnss enu坐标系上
# #ENU_List :名字 e n u 转化为: e n u
# 4-1 读取gnss enu
# 取出前400个数据计算
ENU_GNSS_List_4= API_read2txt("data/test/2ENU_from_GNSS.txt")
ENU_GNSS_List_4_400=[]
for i in range(160 , len(ENU_GNSS_List_4)):
ENU_GNSS_List_4_400.append(ENU_GNSS_List_4[i])
ENU_GNSS_List_3_400=API_data0123_to_data123(ENU_GNSS_List_4_400) # 去掉第一列名字
# 4-2 读取colmap enu
ENU_colmap_list_4= API_read2txt("data/test/colmap_images_t.txt")
ENU_colmap_list_3=API_data0123_to_data123(ENU_colmap_list_4) # 去掉第一列名字
# 4-4 计算变换关系 points_src 到 points_dst
points_src=ENU_colmap_list_3
points_dst=ENU_GNSS_List_3_400 #ENU_GNSS_List_3_400
RT_34, SR, T = API_pose_estimation_3dTo3d_ransac(points_src, points_dst) #
colmapenu_in_gnssenu_3=API_src3D_sRt_dis3D_list(points_src,points_dst,SR, T)
# 4-5 保存计算结果 colmap enu变换到gnss enu坐标系下的新坐标
colmapenu_in_gnssenu_4=[]
for i in range(0,len(ENU_GNSS_List_4_400)):
name=ENU_GNSS_List_4[i][0]
#保存数据 名字 e n u
li=[name,colmapenu_in_gnssenu_3[i][0],colmapenu_in_gnssenu_3[i][1],colmapenu_in_gnssenu_3[i][2]]
colmapenu_in_gnssenu_4.append(li)
# 保存数据
colmapeEnu_from_GnssEnu_txt_name="data/test/3colmapeEnu_from_GnssEnu.txt"
API_Save2txt(colmapeEnu_from_GnssEnu_txt_name,colmapenu_in_gnssenu_4)
'''
