利用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函數如何使用的目的。謝謝大家。