学做文案的网站,网站建设使用工具,商务网站建设的调研流程,做网站的服务器很卡怎么办相对位姿估计
示意图 理论推导
离线数据库#xff1a;
P的位置 P [ X , Y , Z ] T P[X,Y,Z]^{T} P[X,Y,Z]T
相机内参 k 1 k_{1} k1
安卓手机#xff1a;
相机内参 k 2 k_{2} k2
两个像素点位置 #xff1a; p 1 和 p 2 p_1和p_2 p1和p2
公式一#xff1a;…相对位姿估计
示意图 理论推导
离线数据库
P的位置 P [ X , Y , Z ] T P[X,Y,Z]^{T} P[X,Y,Z]T
相机内参 k 1 k_{1} k1
安卓手机
相机内参 k 2 k_{2} k2
两个像素点位置 p 1 和 p 2 p_1和p_2 p1和p2
公式一 s 1 p 1 K 1 P s_1p_1K_1P s1p1K1P s 2 p 2 K 2 ( R P t ) s_2p_2K_2(RPt) s2p2K2(RPt)
**公式二**归一化平面上的坐标 x 1 K 1 − 1 p 1 x_1K_1^{-1}p_1 x1K1−1p1 x 2 K 2 − 1 p 2 x_2K_2^{-1}p2 x2K2−1p2
公式三 x 2 R x 1 t x_2Rx_1t x2Rx1t
公式四 t ^ x 2 t ^ R x 1 \hat{t}x_2\hat{t}Rx_1 t^x2t^Rx1
公式五 x 2 T t ^ x 2 x 2 T t ^ R x 1 x_2^{T}\hat{t}x_2x_2^{T}\hat{t}Rx_1 x2Tt^x2x2Tt^Rx1 x 2 T t ^ R x 1 0 x_2^{T}\hat{t}Rx_10 x2Tt^Rx10
公式六 ( K 2 − 1 p 2 ) T t ^ R K 1 − 1 p 1 (K_2^{-1}p_2)^{T}\hat{t}RK_1^{-1}p_1 (K2−1p2)Tt^RK1−1p1
结论
本质矩阵 E t ^ R E\hat{t}R Et^R ---------------------已知相机参数的情况下
基础矩阵 F K 2 − T E K 1 − 1 FK_2^{-T}EK_1^{-1} FK2−TEK1−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(algorithmFLANN_INDEX_KDTREE, trees5)search_params dict(checks50)flann cv2.FlannBasedMatcher(index_params, search_params)matches flann.knnMatch(descriptors1, descriptors2, k2)# 保留良好的匹配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(fX方向平移: {t[0]}, Y方向平移: {t[1]}, Z方向平移: {t[2]})print(fYaw 角度: {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]], dtypefloat)# 手机相机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]], dtypefloat)# 检测关键点和计算描述符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(algorithmFLANN_INDEX_KDTREE, trees5)search_params dict(checks50)flann cv2.FlannBasedMatcher(index_params, search_params)matches flann.knnMatch(descriptors1, descriptors2, k2)# 保留良好的匹配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(fX方向平移: {t[0]}, Y方向平移: {t[1]}, Z方向平移: {t[2]})print(fYaw 角度: {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]], dtypefloat)# 手机相机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]], dtypefloat)# 检测关键点和计算描述符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秒求解位姿的旋转和平移所需要的时间不多
