企业官网建设流程全解析

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 - 博客园