相对位姿估计

相对位姿估计

示意图

理论推导

离线数据库：

P的位置$$P=[X,Y,Z{]}^T$$

相机内参$$k_1$$

安卓手机：

相机内参$$k_2$$

两个像素点位置 ：$$p_1和p_2$$

公式一：

$$s_1{p}_1=K_{1}P$$ $$s_2{p}_2=K_2(RP+t)$$

**公式二：**归一化平面上的坐标

$$x_1=K_1^{-1}{p}_1$$ $$x_2=K_2^{-1}p2$$

公式三：

$$x_2=R{x}_1+t$$

公式四：

$$\hat{t}{x}_2=\hat{t}R{x}_1$$

公式五

$$x_2^T\hat{t}{x}_2=x_2^T\hat{t}R{x}_1$$

$$x_2^T\hat{t}R{x}_1=0$$

公式六：

$$(K_2^{-1}{p}_2{)}^T\hat{t}R{K}_1^{-1}{p}_1$$

结论：

本质矩阵： $$E=\hat{t}R$$ ---------------------已知相机参数的情况下

基础矩阵： $$F=K_2^{-T}E{K}_1^{-1}$$ -----------未知相机参数的情况下

伪代码

input:image_src,k_src,image_dst,k_dst
output:R,t
1 feature_detect(image_src,image_dst)---->keypoints and deccriptors
2 feature_match(image_src,image_dst)---->matched_features
3 find_essentialmatrix(matched_keypoints,k_src,k_dst)----->essential_matrix
4 decompose_E(essentialmatrix)----->R,t
5 judge "left or right"

实现代码

import cv2
import numpy as npdef find_keypoints_and_descriptors(image):# 使用SIFT算法检测关键点和计算描述符sift = cv2.SIFT_create()keypoints, descriptors = sift.detectAndCompute(image, None)return keypoints, descriptorsdef match_keypoints(descriptors1, descriptors2):# 使用FLANN匹配器进行关键点匹配FLANN_INDEX_KDTREE = 0index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)search_params = dict(checks=50)flann = cv2.FlannBasedMatcher(index_params, search_params)matches = flann.knnMatch(descriptors1, descriptors2, k=2)# 保留良好的匹配good_matches = []for m, n in matches:if m.distance < 0.7 * n.distance:good_matches.append(m)return good_matchesdef estimate_relative_pose(keypoints1, keypoints2, good_matches, camera_matrix_src, camera_matrix_dst):# 提取匹配点对应的关键点src_pts = np.float32([keypoints1[m.queryIdx].pt for m in good_matches]).reshape(-1, 1, 2)dst_pts = np.float32([keypoints2[m.trainIdx].pt for m in good_matches]).reshape(-1, 1, 2)# 使用基础矩阵估计相机的相对位姿essential_matrix, _ = cv2.findEssentialMat(src_pts, dst_pts, camera_matrix_src, None, camera_matrix_dst, None, cv2.RANSAC, 0.999, 1.0)# 从基础矩阵中恢复旋转和平移矩阵_, R, t, _ = cv2.recoverPose(essential_matrix, src_pts, dst_pts, camera_matrix_src)return R, tdef determine_camera_direction(t, R):# 打印平移向量print(f"平移向量 t： {t}")# 计算旋转矩阵的欧拉角angles = cv2.Rodrigues(R)[0]yaw = np.arctan2(angles[1, 0], angles[0, 0]) * 180.0 / np.pi# 联合判断相机的方向if t[0] > 0 and yaw > 0:print("相机偏向右侧, 您应该向左转")elif t[0] < 0 and yaw < 0:print("相机偏向左侧，您应该向右转")elif t[0] > 0 and yaw < 0:print("相机偏向右侧, 但是角度偏向左")elif t[0] < 0 and yaw > 0:print("相机偏向左侧, 但是角度偏向右")else:print("相机方向正前方")print(f"X方向平移: {t[0]}, Y方向平移: {t[1]}, Z方向平移: {t[2]}")print(f"Yaw 角度: {yaw}")def main():# 加载两张图片image1 = cv2.imread('/media/k1928-3/028efb59-765e-462b-8aa6-085565fa80eb/hxy/biaoding/weiziguji/DJI_0273.JPG', cv2.IMREAD_GRAYSCALE)image2 = cv2.imread('/media/k1928-3/028efb59-765e-462b-8aa6-085565fa80eb/hxy/biaoding/weiziguji/phone/ori_right.jpg', cv2.IMREAD_GRAYSCALE)# 假设你已知相机内参----数据库相机fx_src = 4282.03fy_src = 2960.54cx_src = 844.20cy_src = 552.00camera_matrix_src = np.array([[fx_src, 0, cx_src],[0, fy_src, cy_src],[0, 0, 1]], dtype=float)# 手机相机fx_dst = 2934.52fy_dst = 2934.89cx_dst = 1466.29cy_dst = 2020.34camera_matrix_dst = np.array([[fx_dst, 0, cx_dst],[0, fy_dst, cy_dst],[0, 0, 1]], dtype=float)# 检测关键点和计算描述符keypoints1, descriptors1 = find_keypoints_and_descriptors(image1)keypoints2, descriptors2 = find_keypoints_and_descriptors(image2)# 匹配关键点good_matches = match_keypoints(descriptors1, descriptors2)# 估计相机的相对位姿R, t = estimate_relative_pose(keypoints1, keypoints2, good_matches, camera_matrix_src, camera_matrix_dst)# 联合判断相机的方向determine_camera_direction(t, R)if __name__ == "__main__":main()

##八点法

import cv2
import numpy as np
import timedef find_keypoints_and_descriptors(image):# 使用SIFT算法检测关键点和计算描述符sift = cv2.SIFT_create()keypoints, descriptors = sift.detectAndCompute(image, None)return keypoints, descriptorsdef match_keypoints(descriptors1, descriptors2):# 使用FLANN匹配器进行关键点匹配FLANN_INDEX_KDTREE = 0index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)search_params = dict(checks=50)flann = cv2.FlannBasedMatcher(index_params, search_params)matches = flann.knnMatch(descriptors1, descriptors2, k=2)# 保留良好的匹配good_matches = []for m, n in matches:if m.distance < 0.7 * n.distance:good_matches.append(m)return good_matchesdef estimate_relative_pose_eight_point(keypoints1, keypoints2, good_matches, camera_matrix_src, camera_matrix_dst):# 提取匹配点对应的关键点src_pts = np.float32([keypoints1[m.queryIdx].pt for m in good_matches]).reshape(-1, 1, 2)dst_pts = np.float32([keypoints2[m.trainIdx].pt for m in good_matches]).reshape(-1, 1, 2)# 计算基础矩阵fundamental_matrix, _ = cv2.findFundamentalMat(src_pts, dst_pts, cv2.FM_8POINT)# 计算本质矩阵essential_matrix = camera_matrix_dst.T @ fundamental_matrix @ camera_matrix_src# 从本质矩阵中恢复旋转和平移矩阵_, R, t, _ = cv2.recoverPose(essential_matrix, src_pts, dst_pts, camera_matrix_src)return R, tdef determine_camera_direction(t, R):# 打印平移向量print(f"平移向量 t： {t}")# 计算旋转矩阵的欧拉角angles = cv2.Rodrigues(R)[0]yaw = np.arctan2(angles[1, 0], angles[0, 0]) * 180.0 / np.pi# 联合判断相机的方向if t[0] > 0 and yaw > 0:print("人在走廊中轴线左侧, 手机摄像头角度偏右，您应该向右走，应将手机向左偏")elif t[0] < 0 and yaw < 0:print("人在走廊中轴线右侧，手机摄像头角度偏左，您应该向左走，应将手机向右偏")elif t[0] > 0 and yaw < 0:print("人在走廊中轴线左侧, 手机摄像头角度偏左，您应该向右走，应将手机向右偏")elif t[0] < 0 and yaw > 0:print("人在走廊中轴线右侧，手机摄像头角度偏右，您应该向左走，应将手机向左偏")else:print("相机方向正前方")print(f"X方向平移: {t[0]}, Y方向平移: {t[1]}, Z方向平移: {t[2]}")print(f"Yaw 角度: {yaw}")def main():# 加载两张图片image1 = cv2.imread('/media/k1928-3/028efb59-765e-462b-8aa6-085565fa80eb/hxy/biaoding/weiziguji/DJI_0273.JPG', cv2.IMREAD_GRAYSCALE)image2 = cv2.imread('/media/k1928-3/028efb59-765e-462b-8aa6-085565fa80eb/hxy/biaoding/weiziguji/phone/ori_right.jpg', cv2.IMREAD_GRAYSCALE)# 假设你已知相机内参----数据库相机fx_src = 4282.03fy_src = 2960.54cx_src = 844.20cy_src = 552.00camera_matrix_src = np.array([[fx_src, 0, cx_src],[0, fy_src, cy_src],[0, 0, 1]], dtype=float)# 手机相机fx_dst = 2934.52fy_dst = 2934.89cx_dst = 1466.29cy_dst = 2020.34camera_matrix_dst = np.array([[fx_dst, 0, cx_dst],[0, fy_dst, cy_dst],[0, 0, 1]], dtype=float)# 检测关键点和计算描述符keypoints1, descriptors1 = find_keypoints_and_descriptors(image1)keypoints2, descriptors2 = find_keypoints_and_descriptors(image2)# 匹配关键点good_matches = match_keypoints(descriptors1, descriptors2)# 记录开始时间start_time = time.time()# 使用八点法估计相机的相对位姿R, t = estimate_relative_pose_eight_point(keypoints1, keypoints2, good_matches, camera_matrix_src, camera_matrix_dst)# 联合判断相机的方向determine_camera_direction(t, R)# 记录结束时间并计算总时间end_time = time.time()elapsed_time = (end_time - start_time) * 1000  # 转换为毫秒print(f"求解位姿总耗时：{elapsed_time:.6f} 毫秒")if __name__ == "__main__":main()

注意：提取特征的时间是6s，特征匹配的时间是6秒，求解位姿的旋转和平移，所需要的时间不多