一、 思路

文本主要解决如何直接调用python-opencv中的solvepnp函数来实现对位姿解算问题。

现已知有坐标点如下：

           534101.0 3378779.7 33.46 1016 611
           534066.6 3378751.0 33.83 595 457
           534096.9 3378749.8000000003 33.91 737 725
           534060.1 3378779.8000000003 33.45 812 256

以上坐标点格式为：大地坐标系x 大地坐标系y 大地高程h 图片坐标i 图片坐标j

已有四个控制点并将其写在coord.txt中。并新建solvepnp.txt保存最后结果。

二、 代码及分析

直接贴上代码如下：

#!/usr/bin/env pythonimport cv2
import sys
import numpy as np
from osgeo import gdalif __name__ == "__main__":# img2path = sys.argv[1]# coordtxtpath = sys.argv[2]# resultfilename = sys.argv[3]img2path = r'...\img2_resize.tif' # 用于获取照片长宽信息coordtxtpath = r'...\coord.txt' # 匹配控制点坐标信息resultfilename = r'...\solvepnp.txt' # 输出结果文件pointcount = 0# Read Imagegdal.AllRegister()  # 用于获取照片长宽信息dataset = gdal.Open(img2path)size = [dataset.RasterXSize, dataset.RasterYSize]  # 照片长宽信息# Read coordtxtfi = open(coordtxtpath,'r')  # 获取控制点坐标txt = fi.readlines()a = []for w in txt:curLine = w.strip().split(" ")a.append(curLine)pointcount += 1a = np.array(a, dtype=np.double)# 2D image points. If you change the image, you need to change vectorimage_points = a[:, [3, 4]]  # 3D model points.model_points = a[:, [0, 1, 2]]# Camera internalsfocal_length = 3666.666504# 设置相机参数矩阵camera_matrix = np.array(  [[focal_length, 0, size[0] / 2],[0, focal_length, size[1] / 2],[0, 0, 1]], dtype=np.double)# print("Camera Matrix :\n {0}".format(camera_matrix))dist_coeffs = np.zeros((4, 1))  # Assuming no lens distortion(success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points, camera_matrix, dist_coeffs,                           
flags=cv2.SOLVEPNP_ITERATIVE)# print("Rotation Vector:\n {0}".format(rotation_vector))# print("Translation Vector:\n {0}".format(translation_vector))rotM = cv2.Rodrigues(rotation_vector)[0]position = -np.matrix(rotM).T * np.matrix(translation_vector)# print("file Vector:\n {0}".format(-np.matrix(rotM).T * np.matrix(translation_vector)))# print(position[1,0])
#  利用txt传输写出 匹配坐标点数据f2 = open(resultfilename, 'w+')f2.truncate()f2.flush()f2.write(str(position[0,0]) + ' ' + str(position[1,0])+ ' ' + str(position[2,0])+ '\n')f2.flush()

代码中前期工作很多都是为了获取camera_matrix相机参数矩阵的作用，包括img的读取是为了size的获取也是如此。核心  的函数调用是在 cv2.solvePnP(model_points,image_points,camera_matrix,dist_coeffs,flages=cv2.SOLVEPNP_ITERATIVE)的使用，其中flags决定利用cv2中具体哪一种方法，本代码采用的是线性的。具体可以修改为p3p等方法，具体看情况。本文不做探究。

三、结果

最后输出结果为：534081.6833331245 ，3378757.484342166，400.8880741973334

得到的是大地坐标与高程。将其导入地图中可以发现与之前导入的四个点坐标紧密联系，目视判断是正确的。到此也达到了本文阐述opencv中solvePnP函数如何使用的目的。谢谢大家。