多帧三角化原理

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:index2, :] A[0:2, :] self.b[index:index2, :] 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 - 博客园