使用对极约束估计了相机的运动，在得到运动之后，我们需要用相机的运动估计特征点的空间位置。在单目SLAM中，仅仅通过单张图像无法获得像素深度信息，需要通过三角测量来估计其深度。
如图所示，已知相机内参，外参以及匹配点，求解三维空间点的坐标。
原理分析
设世界坐标系下3D点坐标为\(X^w = [x, y, z]^T\),同时被m个相机观测到，第i各相机位姿为\(R^w_{bi}, t^w_{bi}\)归一化坐标系下观测为\(z_i = [u_i, v_i, 1]^T\),深度为\(s_i\)，根据投影方程得到：
\[
s_i \left[\begin{array}{c}u_i\\v_i\\1\end{array}\right] = {R^w_{bi}}^T\left(\left[\begin{array}{c}x\\y\\z\end{array}\right]-t^w_{bi}\right)
=\left[{R^w_{bi}}^T,-{R^w_{bi}}^T t^w_{bi}\right]\left[\begin{array}{c}x\\y\\z\\1\end{array}\right]=P_i X^w = \begin{bmatrix} P^0_i \\ P^1_i \\ P^2_i \end{bmatrix} X^w
\]
其中P为3x4投影矩阵，\(P^j_i\)表示\(P_i\)第j行. 为了消除\(s_i\)，等式两边同时左叉乘\(z_i\)。写成反对称形式：
\[
z_i^\Lambda=\begin{bmatrix}u_i\\v_i\\1\end{bmatrix}^\Lambda=\begin{bmatrix}0&-1&v_i\\1&0&-u_i\\-v_i&u_i&0\end{bmatrix}
\]
得到：
\[
\begin{bmatrix}
-P_i^1+v_iP_i^2\\
P_i^0-u_iP_i^2\\
-v_iP_i^0+u_iP_i^1
\end{bmatrix}
X=0
\]
由于第三个等式是前面两个的线性组合，因此一个点对应只能提供两个线性无关的方程组，重新整理写成如下形式：
\[
\begin{bmatrix}
u_iP_i^2 - P_i^0 \\
v_iP_i^2 - P_i^1
\end{bmatrix}X=0
\]
由于三维点的自由度为3，因此至少需要两个观测才能求解出三维点，假设有m个相机观测，可以得到:
\[
\begin{bmatrix}u_1P_1^2-P_1^0\\v_1P_1^2-P_1^1\\\vdots\\u_mP_m^2-P_m^0\\v_mP_m^2-P_m^1\end{bmatrix}X=AX=\mathbf{0}
\]
Ransac 求解
对于两个视图，多对匹配点得情况，可以使用rasanc剔除外点，求解精确得解。
ransac三角测量得方法步骤是：
- 随机挑选一对视角匹配点，
- 计算三维空间点坐标，
- 重投影到视图，计算重投影误差，误差较大得认为是外点。
- 重复上面过程，挑选最佳内点，并计算点云。
误差分析
三角平移是由平移得到的，有平移才会有对极几何约束的三角形。因此，纯旋转是无法使用三角测量的，对极约束将永远满足。在平移时，三角测量有不确定性，会引出三角测量的矛盾。
对于三角测量，平移较小时误差较大，平移较大时误差越小，因此在构建三角测量时可以选择基线较大得视图。
Code
#include <iostream>
#include <random>
#include <vector>
#include <memory>
#include <algorithm>
#include <Eigen/Eigen>
#include <opencv2/opencv.hpp>
bool reproject(const Eigen::Matrix4d &transform, const Eigen::Matrix3d &intrinsic, const Eigen::Vector4d &object, Eigen::Vector3d &point)
{
point = (intrinsic * ((transform * object).block<3, 1>(0, 0))) / object.z();
return true;
}
bool reproject(const Eigen::Matrix4d &transform, const Eigen::Matrix3d &intrinsic, const std::vector<Eigen::Vector4d> &objects, std::vector<Eigen::Vector3d> &points)
{
points.clear();
for (auto &object : objects) {
Eigen::Vector3d point;
if (!reproject(transform, intrinsic, object, point)) {
continue;
}
points.push_back(point);
}
return true;
}
bool triangulate(const Eigen::Matrix4d &left_transform, const Eigen::Matrix4d &right_transform, const Eigen::Vector3d &left_point, const Eigen::Vector3d &right_point, Eigen::Vector4d &object)
{
Eigen::Matrix4d matrix = Eigen::Matrix4d::Zero();
matrix.row(0) = left_point[0] * left_transform.row(2) - left_transform.row(0);
matrix.row(1) = left_point[1] * left_transform.row(2) - left_transform.row(1);
matrix.row(2) = right_point[0] * right_transform.row(2) - right_transform.row(0);
matrix.row(3) = right_point[1] * right_transform.row(2) - right_transform.row(1);
Eigen::Vector4d point = matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();
object(0) = point(0) / point(3);
object(1) = point(1) / point(3);
object(2) = point(2) / point(3);
object(3) = 1.;
return true;
}
bool triangulate(const Eigen::Matrix4d &left_transform, const Eigen::Matrix4d &right_transform, const std::vector<Eigen::Vector3d> &left_points, const std::vector<Eigen::Vector3d> &right_points, std::vector<Eigen::Vector4d> &objects)
{
objects.clear();
if (left_points.size() != right_points.size()) {
return false;
}
for (auto pos = 0; pos < left_points.size(); pos ++) {
Eigen::Vector4d object;
if (!triangulate(left_transform, right_transform, left_points.at(pos), right_points.at(pos), object)) {
continue;
}
objects.push_back(object);
}
return true;
}
int main(int argc, char * argv[])
{
std::uint16_t object_size = 5;
std::vector<Eigen::Vector4d> object_points;
Eigen::Vector3d translation = Eigen::Vector3d(0., 0., 4.);
Eigen::Isometry3d transform = Eigen::Isometry3d::Identity();
transform.translation() = translation;
std::srand((unsigned int)std::time(nullptr));
std::cout << "object_points: " << std::endl;
for (auto pos = 0; pos < object_size; pos++) {
Eigen::Vector4d point(0., 0., 0., 1.);
point.x() = (float)(std::rand() % 20000 - 10000) / 10000.;
point.y() = (float)(std::rand() % 20000 - 10000) / 10000.;
point.z() = (float)(std::rand() % 20000 - 10000) / 10000.;
point = transform * point;
std::cout << point.transpose() << std::endl;
object_points.push_back(point);
}
Eigen::Matrix3d intrinsic;
intrinsic << 500., 0., 752 / 2., 0., 500., 480 / 2., 0., 0., 1.;
std::cout << "intrinsic: " << std::endl << intrinsic << std::endl;
// left camera
std::vector<Eigen::Vector3d> left_points;
Eigen::Matrix4d left_pose = Eigen::Matrix4d::Identity();
left_pose.block<3, 1>(0, 3) = Eigen::Vector3d(1., 0., 0.);
reproject(left_pose, intrinsic, object_points, left_points);
std::cout << "left: " << std::endl;
for (auto &point : left_points) {
std::cout << point.transpose() << std::endl;
}
// right camera
std::vector<Eigen::Vector3d> right_points;
Eigen::Matrix4d right_pose = Eigen::Matrix4d::Identity();
right_pose.block<3, 1>(0, 3) = Eigen::Vector3d(-1., 0., 0.);
reproject(right_pose, intrinsic, object_points, right_points);
std::cout << "right: " << std::endl;
for (auto &point : right_points) {
std::cout << point.transpose() << std::endl;
}
// triangulate
std::vector<Eigen::Vector4d> objects;
for (auto &point : left_points) {
point = intrinsic.inverse() * point;
}
for (auto &point : right_points) {
point = intrinsic.inverse() * point;
}
triangulate(left_pose, right_pose, left_points, right_points, objects);
std::cout << "objects: " << std::endl;
for (auto &object : objects) {
std::cout << object.transpose() << std::endl;
}
return 0;
}
Reference
Triangulation. Richard I. Hartley and Peter Sturm. GE-CRD, Schenectady, NY. Lifia-Inria, Grenoble
https://users.cecs.anu.edu.au/~hartley/Papers/triangulation/triangulation.pdf
https://perception.inrialpes.fr/Publications/1997/HS97/HartleySturm-cviu97.pdf
https://www.uio.no/studier/emner/matnat/its/UNIK4690/v16/forelesninger/lecture_7_2-triangulation.pdf