利用python-opencv中solvepnp函数解决位姿定位定向问题。

一、 思路

文本主要解决如何直接调用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 python

import cv2
import sys
import numpy as np
from osgeo import gdal

if __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 Image
    gdal.AllRegister()  # 用于获取照片长宽信息
    dataset = gdal.Open(img2path)
    size = [dataset.RasterXSize, dataset.RasterYSize]  # 照片长宽信息

    # Read coordtxt
    fi = open(coordtxtpath,'r')  # 获取控制点坐标
    txt = fi.readlines()
    a = []
    for w in txt:
        curLine = w.strip().split(" ")
        a.append(curLine)
        pointcount += 1
    a = np.array(a, dtype=np.double)

    # 2D image points. If you change the image, you need to change vector
    image_points = a[:, [3, 4]]  
    # 3D model points.
    model_points = a[:, [0, 1, 2]]

    # Camera internals

    focal_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函数如何使用的目的。谢谢大家。