ContextCapture Master的photo positions(相机位姿文件)格式转换

 ContextCapture软件要求的相机position文件格式

来自Import photo positions (bentley.com)

 个人理解：取Easting/Northing/Height为X/Y/Z轴的正方向，则Omega/Phi/Kappa分别对应绕X轴/Y轴/Z轴的旋转

从世界坐标系下的相机外参R/T到Easting/Northing/Height/Omega/Phi/Kappa的转换

python代码，输入旋转矩阵R和平移向量T，输出Easting/Northing/Height/Omega/Phi/Kappa。 来自Rotation Matrix To Euler Angles | LearnOpenCV #

import numpy as np

# Checks if a matrix is a valid rotation matrix.
# 根据旋转矩阵的正交性判断一个矩阵是否为正交矩阵
def isRotationMatrix(R):
    Rt = np.transpose(R)
    shouldBeIdentity = np.dot(Rt, R)
    I = np.identity(3, dtype = R.dtype)
    n = np.linalg.norm(I - shouldBeIdentity)
    return n < 1e-6
 
# Calculates rotation matrix to euler angles
# The result is the same as MATLAB except the order
# of the euler angles ( x and z are swapped ).
def rotationMatrixToEulerAngles(R: np.array, T:np.array) :
    """
    :para R:输入旋转矩阵R
    :para R:输入平移向量矩阵R
    :return e,n,h分别对应Easting,northing,height；x,y,z分别对应omega,phi,kappa
    """
    assert(isRotationMatrix(R))
    sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])
    singular = sy < 1e-6
    if  not singular :
        x = math.atan2(R[2,1] , R[2,2])
        y = math.atan2(-R[2,0], sy)
        z = math.atan2(R[1,0], R[0,0])
    else :
        x = math.atan2(-R[1,2], R[1,1])
        y = math.atan2(-R[2,0], sy)
        z = 0
    e, n, h = T[0], T[1], T[2]
    return np.array([e, n, h, x, y, z]) 

从colmap输出的外参到ContextCapture需要的格式

使用colmap软件输出进行三维重建后，使用PatchmatchNet的github项目(PatchmatchNet/colmap_input.py at main · FangjinhuaWang/PatchmatchNet · GitHub)中的colmap_input.py进行格式转换，得到的...cam.txt中就包括外参extrinsic和相机内参intrinsic

python代码 从...cam.txt中读取外参

def get_RT_from_txt(txt_path):
    """
    :para txt_path输入的...cam.txt文件路径
    """
    with open(txt_path, 'r') as f0:
        lines = f0.readlines()
        for idx, line in enumerate(lines):
            if line == 'extrinsic\n':
                nums_temp = re.findall(r'-?\d*\.?\d*e?-?\d+', lines[idx + 1] + lines[idx + 2] + lines[idx + 3])
                assert len(nums_temp) == 12, len(nums_temp)
                assert lines[idx + 4] == '0.0 0.0 0.0 1.0 \n'
                nums_temp = [float(num) for num in nums_temp]
                nums_temp = np.array(nums_temp).reshape((3, 4))
                R_colmap, T_colmap = nums_temp[:3, :3], nums_temp[:, 3]
                return R_colmap, T_colmap

注意：colmap得到的R/T矩阵是世界坐标系向相机坐标系的转换关系，而非传统的相机坐标系转世界坐标系的关系，而ContextCapture要求的输入是在世界坐标系下的，详见【踩坑记录】colmap中的相机位姿的坐标系定义及其可视化结果的隐含转换_colmap 可视化位姿_zeeq_的博客-CSDN博客因此最后的R/T需要如下换算

R, T = np.transpose(R_colmap), -np.dot(np.transpose(R_colmap), T_colmap)

全部python代码

import copy
import math
import os
import re
import shutil
import cv2
import numpy as np


# Checks if a matrix is a valid rotation matrix.
def isRotationMatrix(R):
    Rt = np.transpose(R)
    shouldBeIdentity = np.dot(Rt, R)
    I = np.identity(3, dtype=R.dtype)
    n = np.linalg.norm(I - shouldBeIdentity)
    return n < 1e-6


# Calculates rotation matrix to euler angles
# The result is the same as MATLAB except the order
# of the euler angles ( x and z are swapped ).
def rotationMatrixToEulerAngles(R):
    assert (isRotationMatrix(R))

    sy = math.sqrt(R[2, 1] * R[2, 1] + R[2, 2] * R[2, 2])

    singular = sy < 1e-6

    if not singular:
        x = math.atan2(R[2, 1], R[2, 2])
        y = math.atan2(-R[2, 0], sy)
        z = math.atan2(R[1, 0], R[0, 0])
    else:
        x = math.atan2(-R[1, 2], R[1, 1])
        y = math.atan2(-R[2, 0], sy)
        z = 0

    return np.array([x, y, z])


def get_r_from_txt(txt_path):
    with open(txt_path, 'r') as f0:
        lines = f0.readlines()
        for idx, line in enumerate(lines):
            if line == 'extrinsic\n':
                nums_temp = re.findall(r'-?\d*\.?\d*e?-?\d+', lines[idx + 1] + lines[idx + 2] + lines[idx + 3])
                assert len(nums_temp) == 12, len(nums_temp)
                assert lines[idx + 4] == '0.0 0.0 0.0 1.0 \n'
                nums_temp = [float(num) for num in nums_temp]
        return np.array(nums_temp).reshape((3, 4))


if __name__ == '__main__':
    # # 测试函数是否正确
    # a, b, c = np.random.rand(3)*1.5
    # print(a,b,c)
    # R = np.array([[math.cos(b)*math.cos(c), math.sin(a)*math.sin(b)*math.cos(c)-math.cos(a)*math.sin(c), math.cos(a)*math.sin(b)*math.cos(c)+math.sin(a)*math.sin(c)],
    #               [math.cos(b)*math.sin(c), math.sin(a)*math.sin(b)*math.sin(c)+math.cos(a)*math.cos(c), math.cos(a)*math.sin(b)*math.sin(c)-math.sin(a)*math.cos(c)],
    #               [-math.sin(b), math.sin(a)*math.cos(b), math.cos(a)*math.cos(b)]])
    # print(rotationMatrixToEulerAngles(R))

    # 将colmap输出外参转为需要的格式，并提取图片。其中cam是存放所有...cam.txt的文件夹，images2是存放对应图片的文件夹，txt_new是最后得到的ContextCapture的输入文件
    txt_list = os.listdir('cams')
    txt_new = 'txt_cam.txt'
    img_list = os.listdir('images2')
    # img_new = 'images2'
    with open(txt_new, 'w') as f:
        f.write('')
    for idx_txt, txt_file in enumerate(txt_list):
        txt_file2 = os.path.join('cams', txt_file)
        print(txt_file2)
        mat_tempt = get_r_from_txt(txt_file2)
        R, T = mat_tempt[:3, :3], mat_tempt[:, 3]
        print('R_colmap')
        print(R)
        print('T_colmap')
        print(T)
        # 见https://blog.csdn.net/weixin_44120025/article/details/124604229
        R, T = np.transpose(R), -np.dot(np.transpose(R), T)
        # print('R')
        # print(R)
        # print('T')
        # print(T)
        Easting, Northing, Height = T[0], T[1], T[2]
        Omega, Phi, Kappa = rotationMatrixToEulerAngles(R)
        # print('Omega, Phi, Kappa')
        # print(Omega, Phi, Kappa)

        # 写入文件
        with open(txt_new, 'a') as f:
            photo_file = r'D:\locate\data\\' + photo_file
            print(photo_file)
            str_temp = ''
            for i in [Easting, Northing, Height, Omega, Phi, Kappa]:
                str_temp = str_temp + ' ' + str(i)
            f.write(photo_file + str_temp + '\n')