三角测量的一些基础理论

三角测量，通俗一点讲就是通过在一个相机在两个不同的位置（或者双目相机）对一个目标点进行观测，依靠两个观测系间的绝对（或相对）位姿变换关系，算出该被观测点在两个观测系中的绝对（或相对）深度。进一步推广，当我们在n个不同的位置对同一个点进行观测，并已知这些观测系相对于一固定系的变换时，便可通过求解超定方程组，求出在该固定系下被观测点坐标的最优解。

构建超定方程组

设待观测点P在世界系的坐标为 P ( X , Y , Z ) P(X,Y,Z) P(X,Y,Z)，我们对它进行多次观测。在产生第n次观测时，相机系与世界系之间的变换表示为 T c w _ n = { R , t } T_{cw\_n}=\{R,t\} Tcw_n={R,t}(后面简写作 T n T_{n} Tn)，这个观测表示为该相机系下的其次坐标 p n ( x , y , 1 ) p_{n}(x,y,1) pn(x,y,1)。此时我们可以得到 T n P ‾ = λ n ( x y 1 ) T_{n} \overline{P} =λ_{n} \left( \begin{array}{c} x \\ y \\ 1 \end{array} \right) TnP=λn⎝⎛xy1⎠⎞其中 λ n λ_{n} λn是在当前相机系被观测点的深度, P ‾ \overline{P} P是P的齐次坐标。在此基础上，我们可以非常容易得出
T n 1 P ‾ − x T n 3 P ‾ = 0 T_{n1}\overline{P}-xT_{n3}\overline{P}=0 Tn1P−xTn3P=0 T n 2 P ‾ − y T n 3 P ‾ = 0 T_{n2}\overline{P}-yT_{n3}\overline{P}=0 Tn2P−yTn3P=0其中 T n i T_{ni} Tni代表T矩阵的第i行。
将待求变量分离出来，改写为 ( T n 1 − x T n 3 T n 2 − y T n 3 ) P ‾ = 0 \left( \begin{array}{c} T_{n1}-xT_{n3} \\ T_{n2}-yT_{n3} \end{array} \right) \overline{P}=0 (Tn1−xTn3Tn2−yTn3)P=0括号内是一个2x4的矩阵，当我们有了多个观测时，便可以构造出一个2n*4的方程组。

解的分析

下面我们来分析一下我们能够得到什么样的解，首先我们从几何角度上看：当我们若只有一个观测时，因为深度未知，所以P可能出现在沿当前观测系下深度方向上的任意一点；当有两个或两个以上不同位置的观测（这决定了方程组有效方程的个数）时，该点在给定系下的绝对位置便可以确定了，但由于观测误差的存在，我们这里一般通过两个以上的观测求得一个最优解。
从齐次方程组的性质上分析：对于Ax=0（其中A是m*n的矩阵），我们通常先构造 A T A x = 0 A^{T}Ax=0 ATAx=0。若 d e t ( A T A ) ≠ 0 det\left(A^{T}A\right)\not=0 det(ATA)=0，也就是说矩阵 A T A A^{T}A ATA满秩，此时只有零解；当 d e t ( A T A ) = 0 det\left(A^{T}A\right)=0 det(ATA)=0时，有无数多组线性相关的解。因为待求解的向量空间只有三自由度（齐次坐标最后一维是1），所以我们只取一组即可，通常求取 ∣ x ∣ = 1 \left|x\right|=1 ∣x∣=1的解再进行归一化。
接下来我们谈一谈这两个角度的一些联系，但在往下继续讲之前，我们先再简单说一下零空间的问题：矩阵 A T A A^{T}A ATA的零空间，对应了方程 A T A x = 0 A^{T}Ax=0 ATAx=0的所有解的集合。因为前文已经提到了，齐次坐标的自由度是3，所以首先， A T A A^{T}A ATA的零空间维数至少为1，此时方程不会只有零解。为什么说至少为1呢？ 此时对应我们前面在几何角度的分析，如果我们只拥有一个观测的话，那么在空间中便无法有多条射线形成一个交点，在其对应观测系下的深度我们是可以任意取值的。这就造成了系统状态空间不可观的维度（即零空间的维度）又加了1，唯一解退化成了无数个线性相关（与观测系下深度线性相关）的解。那么对于两个及以上的观测，零空间为1，求解齐次坐标可通过对 A T A A^{T}A ATA进行SVD，找出最小的那个奇异值（往往特别特别小和次小相比可忽略不计），所对应的特征向量便是我们所需要的解。

#include <iostream>
#include <vector>
#include <random>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <fstream>struct Pose
{Pose(Eigen::Matrix3d R, Eigen::Vector3d t):Rwc(R),qwc(R),twc(t) {};Eigen::Matrix3d Rwc;Eigen::Quaterniond qwc;Eigen::Vector3d twc;Eigen::Vector2d uv;
};
int main()
{int poseNums = 10;double radius = 8;double fx = 1.;double fy = 1.;std::vector<Pose> camera_pose;for(int n = 0; n < poseNums; ++n ) {double theta = n * 2 * M_PI / ( poseNums * 4); // 绕 z轴 旋转Eigen::Matrix3d R;R = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());Eigen::Vector3d t = Eigen::Vector3d(radius * cos(theta) - radius, radius * sin(theta), 1 * sin(2 * theta));camera_pose.push_back(Pose(R,t));}// 随机数生成 1 个 三维特征点std::default_random_engine generator;std::uniform_real_distribution<double> xy_rand(-4, 4.0);std::uniform_real_distribution<double> z_rand(8., 10.);double tx = xy_rand(generator);double ty = xy_rand(generator);double tz = z_rand(generator);std::cout << tx << " " << ty << " " << tz << std::endl;std::default_random_engine noise_generator;double variance;variance = 0.0;std::normal_distribution<double> noise_pdf(0, variance);double noise;Eigen::Vector3d Pw(tx, ty, tz);std::cout << "generate 3D point finished" << std::endl;int start_frame_id = 0;int end_frame_id = 1;for (int i = start_frame_id; i < end_frame_id; ++i) {Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();Eigen::Vector3d Pc = Rcw * (Pw - camera_pose[i].twc);noise = noise_pdf(noise_generator);std::cout << noise << std::endl;double x = Pc.x() + noise;noise = noise_pdf(noise_generator);std::cout << noise << std::endl;double y = Pc.y() + noise;noise = noise_pdf(noise_generator);std::cout << noise << std::endl << std::endl;double z = Pc.z() + noise;camera_pose[i].uv = Eigen::Vector2d(x/z,y/z);}std::cout << "measurement generated" << std::endl;Eigen::Vector3d P_est;         int D_row = 2 * (end_frame_id - start_frame_id);Eigen::MatrixXd D;D.resize(D_row, 4);for(int i = 0; i < D_row/2; i++){//对应camera_pose[start_frame+i]Eigen::MatrixXd T(3,4);Eigen::Matrix3d rotation = camera_pose[i+start_frame_id].Rwc.transpose();Eigen::Vector3d trans = -1 * rotation * camera_pose[i+start_frame_id].twc;for(int j = 0; j < 3; j++)T.block(j, 0, 1, 4) << rotation(j,0), rotation(j,1), rotation(j,2), trans(j);std::cout << "T:" << std::endl << T << std::endl;D.block(i*2, 0, 1, 4) = camera_pose[i+start_frame_id].uv(0) * T.block(2, 0, 1, 4) - T.block(0, 0, 1, 4);D.block(i*2+1,0,1, 4) = camera_pose[i+start_frame_id].uv(1) * T.block(2, 0, 1, 4) - T.block(1, 0, 1, 4);}std::cout << "D:" << std::endl << D << std::endl;Eigen::MatrixXd square(4,4);square = D.transpose() * D;Eigen::JacobiSVD<Eigen::MatrixXd> svd(square, Eigen::ComputeThinU | Eigen::ComputeThinV);std::cout << "Singular values: " << std::endl << svd.singularValues() << std::endl;std::cout << "matrix U: " << std::endl << svd.matrixU() << std::endl;std::cout << "matrix v: " << std::endl << svd.matrixV() << std::endl;std::cout <<"ground truth: \n"<< Pw.transpose() <<std::endl;double ddd = svd.matrixU()(1,3);P_est << svd.matrixU()(0,3)/svd.matrixU()(3,3), svd.matrixU()(1,3)/svd.matrixU()(3,3), svd.matrixU()(2,3)/svd.matrixU()(3,3); std::cout <<"my result: \n"<< P_est.transpose() <<std::endl;return 0;
}

以上代码提供了一个简单的三角化例程，在生成了一个三维点并只用一个相机对他进行观测。可以发现四个奇异值两个都极小，接近于零，这也印证了我们上文的分析，这种状况下待求解的变量空间的不可观维数是2。

（如有需要转发，请注明出处～）