多帧三角化原理

1. 概述

多帧三角化就是在已知多帧图像2D观测，相机内参矩阵K，以及每个观测图像帧的相机位姿（从世界坐标系到相机坐标系，相机坐标系是右下前，即光心与图像中心的连线是Z轴，图像的宽度方向是X轴，图像高度方向是Y轴）的情况下求解出3D点在世界坐标系下的位置p的过程，这里的多帧。

2. 原理

一个3D点在每个图像观测帧中有下面公式（1）所示的约束，公式（1）等式左边是2D观测的归一化坐标，归一化坐标可以通过公式（3）求得，s是在该相机坐标系下3D点的深度信息，P是3D点的齐次形式。

多帧三角化还是用到了构建超定方程或者的形式来求解，下面就讲怎么构建这两种形式。

2.1. 齐次方法

先讲的形式，对公式（1）中用进行表示，，，，表示该矩阵的第1、2、3行。因此对公式（1）进行变型可以得到公式（4）。

然后对公式（4）两边都叉乘2D观测的归一化坐标，得到公式（5)，其中叉乘可以通过公式（6）表示，然后通过公式（5）可以得到公式（7）所示三个等式，用矩阵表示这三个等式就是公式（8）所示的矩阵，因为公式（8）所示的矩阵中的三行只有两行是线性独立的，所以只用其中任意两行就构成了超定方程A中的两行，当有n个观测帧时就可以构建成一个的A矩阵。

那么A矩阵做奇异值分解后最小奇异值对应的单位向量V就是就解，但是这里有一个比较特殊的就是P是一个齐次形式，因此需要将V齐次化（），这样就可以得到p。当对含有观测噪声的数据做最小二乘求解时，求得的可能是一个非常接近于0的值，那么算出的p点就是一个无穷远的点。因此齐次方法可能会存在这样的问题。那下面看一下非齐次的方法。

2.2. 非齐次方法

非齐次方法就是对上面公式（5）变型成另外的形式，将用代替，P用代替，那么我们就可以得到公式（9）所示的三个等式，然后将其整理成公式（10），最后得到公式（11）所示的矩阵形式，因为同样公式（11）的三行中只有两行是线性独立的，所以选择公式（11）中任意两行构成超定方程中的两行，当有n个观测帧时就可以构建成一个的 A矩阵，和的b矩阵。因此得到最小二乘解就是3D点p。

3. 代码

import numpy as np
import math

class Triangulation:
    def __init__(self, camera_matrix):
        # calibration matrix
        self._camera_matrix = camera_matrix
        self.projections = {}
        for (camera_name, _) in camera_matrix.items():
            self.projections[camera_name] = []

        self.A = None  # Ax = b
        self.b = None

    def add_projection(self, camera_name, pose_camera_world, projection):
        self.projections[camera_name].append((pose_camera_world, projection))

    def compute_error(self, Pw):
        errors = []
        for (camera_name, projections) in self.projections.items():
            for (pose_camera_world, projection) in projections:
                K = self._camera_matrix[camera_name]
                R = pose_camera_world[0:3, 0:3]
                t = pose_camera_world[0:3, 3:4]
                Pc = np.matmul(R, Pw) + t
                Pc[0][0] /= Pc[2][0]
                Pc[1][0] /= Pc[2][0]
                Pc[2][0] = 1
                Puv = np.matmul(K, Pc)
                Euv = Puv - np.array([[projection[0]], [projection[1]], [1]])
                error = np.matmul(np.transpose(Euv), Euv)[0][0]
                errors.append(math.sqrt(error))
        return errors

    def triangulate(self):
        number_projections = 0
        for (camera_name, projections) in self.projections.items():
            number_projections += len(projections)

        if number_projections < 2:
            return np.array([[0], [0]])

        # assign memory
        self.A = np.zeros((number_projections * 2, 3))
        self.b = np.zeros((number_projections * 2, 1))

        index = 0
        for (camera_name, projections) in self.projections.items():
            for (pose_camera_world, projection) in projections:
                (A, b) = self._compute_equation(camera_name, pose_camera_world, projection)
                self.A[index:index+2, :] = A[0:2, :]
                self.b[index:index+2, :] = b[0:2, :]
                index += 2
        At = np.transpose(self.A)
        AtA = np.matmul(At, self.A)
        Atb = np.matmul(At, self.b)
        return np.matmul(np.linalg.inv(AtA), Atb)

    def skew(self, v):
        x = v[0][0]
        y = v[1][0]
        z = v[2][0]
        m = np.array([[0., -z, y], [z, 0., -x], [-y, x, 0.]])
        return m

    def _compute_equation(self, camera_name, pose_camera_world, projection):
        K = self._camera_matrix[camera_name]
        R = pose_camera_world[0:3, 0:3]
        t = pose_camera_world[0:3, 3:4]
        Puv = np.array([[projection[0]], [projection[1]], [1]])
        Pc = np.matmul(np.linalg.inv(K), Puv)
        Pcx = self.skew(Pc)
        A = np.matmul(-Pcx, R)
        b = np.matmul(Pcx, t)
        return A, b

参考文献

多帧三角化原理 - 知乎

三角化 - MKT-porter - 博客园