单目初始化 2

欢迎访问我的博客首页。

---

6. 三角测量

---

6.1 使用归一化坐标

---

  假设世界坐标系 w 中的坐标到相机坐标系 i，j 的坐标的变换分别是 ${\mathbf{T}}_{iw}$，${\mathbf{T}}_{jw}$。已知世界坐标系中的一点 ${\mathbf{p}}_w$ 在相机坐标系 i，j 中的坐标分别是 ${\mathbf{p}}_i=s_i[u_i,v_i,1{]}^T$，${\mathbf{p}}_j=s_j[u_j,v_j,1{]}^T$。求 ${\mathbf{p}}_w$。

  由题设知

$$\{\begin{array}{rl}{s}_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]& ={\mathbf{T}}_{iw}{\mathbf{p}}_w\\ s_j\left[\begin{array}{c}{u}_j\\ v_j\\ 1\end{array}\right]& ={\mathbf{T}}_{jw}{\mathbf{p}}_w\end{array}\text{\hspace{1em}(6.1)}$$

写成方程组的形式为

$$\{\begin{array}{rl}{s}_i{u}_i& ={\mathbf{T}}_{iw}.row(0)\cdot {\mathbf{p}}_w\\ s_i{v}_i& ={\mathbf{T}}_{iw}.row(1)\cdot {\mathbf{p}}_w\\ s_i& ={\mathbf{T}}_{iw}.row(2)\cdot {\mathbf{p}}_w\\ s_j{u}_j& ={\mathbf{T}}_{jw}.row(0)\cdot {\mathbf{p}}_w\\ s_j{v}_j& ={\mathbf{T}}_{jw}.row(1)\cdot {\mathbf{p}}_w\\ s_j& ={\mathbf{T}}_{jw}.row(2)\cdot {\mathbf{p}}_w\end{array}$$

把第 3 个等式代入第 1，2 个等式，把第 6 个等式代入第 4，5 个等式得

$$\{\begin{array}{rl}[{\mathbf{T}}_{iw}.row(2)\cdot u_i-{\mathbf{T}}_{iw}.row(0)]\cdot {\mathbf{p}}_w& =0\\ [{\mathbf{T}}_{iw}.row(2)\cdot v_i-{\mathbf{T}}_{iw}.row(1)]\cdot {\mathbf{p}}_w& =0\\ [{\mathbf{T}}_{jw}.row(2)\cdot u_j-{\mathbf{T}}_{jw}.row(0)]\cdot {\mathbf{p}}_w& =0\\ [{\mathbf{T}}_{jw}.row(2)\cdot v_j-{\mathbf{T}}_{jw}.row(1)]\cdot {\mathbf{p}}_w& =0\end{array}$$

求解该齐次线性方程组即可得到 ${\mathbf{p}}_w$。一种方法是对其系数矩阵 A 奇异值分解 $\mathbf{A}=\mathbf{U\Lambda }{\mathbf{V}}^{\mathbf{T}}$，则矩阵 $\mathbf{V}$ 的最后一列即为该方程组的解。

  下面是 ORB_SLAM 和 vins_mono 实现的三角测量。

void Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D) {
    cv::Mat A(4, 4, CV_32F);
    A.row(0) = kp1.pt.x * P1.row(2) - P1.row(0);
    A.row(1) = kp1.pt.y * P1.row(2) - P1.row(1);
    A.row(2) = kp2.pt.x * P2.row(2) - P2.row(0);
    A.row(3) = kp2.pt.y * P2.row(2) - P2.row(1);
    cv::Mat u, w, vt;
    cv::SVD::compute(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
    x3D = vt.row(3).t();
    x3D = x3D.rowRange(0, 3) / x3D.at<float>(3);
}

我们可以使用一组归一化坐标测试

int main() {
    // 1.相机 i 的位姿 Tiw。
    cv::Mat Tiw = cv::Mat::eye(3, 4, CV_32F);
    // 2.相机 j 的位姿 Tjw。
    cv::Mat R = (cv::Mat_<float>(3, 3) << 0, 1, 0, -1, 0, 0, 0, 0, 1);
    cv::Mat t = (cv::Mat_<float>(3, 1) << 0, -1, 0);
    cv::Mat Tjw(3, 4, CV_32F);
    R.copyTo(Tjw.rowRange(0, 3).colRange(0, 3));
    t.copyTo(Tjw.rowRange(0, 3).col(3));
    // 3.归一化坐标 ni, nj。
    cv::KeyPoint ni(-4, 2, 1);
    cv::KeyPoint nj(2, 3, 1);
    // 4.三角测量。
    cv::Mat pw;
    Triangulate(ni, nj, Tiw, Tjw, pw);
    std::cout << pw.t() << std::endl;           // [-4, 2, 1]。
    std::cout << (R * pw + t).t() << std::endl; // [2, 3, 1]。
}

6.2 使用像素坐标

---

  对公式 (6.1) 的等式两边同时左乘相机内参矩阵，等式仍然成立。归一化坐标左侧乘以内参矩阵得到像素坐标，位姿左侧乘以内参矩阵得到投影矩阵。所以该三角测量函数也可以接收像素坐标和投影矩阵。

// 相机坐标系到像素坐标系的变换。
cv::Point2f cam2pix(const cv::Point3f &p, const cv::Mat &K) {
    return cv::Point2f((p.x * K.at<float>(0, 0)) / p.z + K.at<float>(0, 2), (p.y * K.at<float>(1, 1)) / p.z + K.at<float>(1, 2));
}

int main() {
    // 1.相机 i 的内参 Ki 和位姿 Tiw。
    cv::Mat Ki = (cv::Mat_<float>(3, 3) << 521.0, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    cv::Mat Tiw = cv::Mat::eye(3, 4, CV_32F);
    // 2.相机 j 的内参 Kj 和位姿 Tjw。
    cv::Mat Kj = (cv::Mat_<float>(3, 3) << 521.0, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    cv::Mat R = (cv::Mat_<float>(3, 3) << 0, 1, 0, -1, 0, 0, 0, 0, 1);
    cv::Mat t = (cv::Mat_<float>(3, 1) << 0, -1, 0);
    cv::Mat Tjw(3, 4, CV_32F);
    R.copyTo(Tjw.rowRange(0, 3).colRange(0, 3));
    t.copyTo(Tjw.rowRange(0, 3).col(3));
    // 3.一对像素坐标。
    cv::KeyPoint kpi(cam2pix(cv::Point3f(-4, 2, 1), Ki), 1);
    cv::KeyPoint kpj(cam2pix(cv::Point3f(2, 3, 1), Kj), 1);
    // 4.三角测量。
    cv::Mat pw;
    Triangulate(kpi, kpj, Ki * Tiw, Kj * Tjw, pw);
    std::cout << pw.t() << std::endl;           // [-4, 2, 1]。
    std::cout << (R * pw + t).t() << std::endl; // [2, 3, 1]。
}

6.3 使用 OpenCV

---

  直接调用 OpenCV 的三角测量函数 triangulatePoints。对于归一化坐标

// 像素坐标系到归一化成像平面的变换。
cv::Point2f cam2nor(const cv::Point3f &p) { return cv::Point2f(p.x / p.z, p.y / p.z); }

int main() {
    // 1.相机 i 的位姿 Tiw。
    cv::Mat Tiw = cv::Mat::eye(3, 4, CV_32F);
    // 2.相机 j 的位姿 Tjw。
    cv::Mat R = (cv::Mat_<float>(3, 3) << 0, 1, 0, -1, 0, 0, 0, 0, 1);
    cv::Mat t = (cv::Mat_<float>(3, 1) << 0, -1, 0);
    cv::Mat Tjw(3, 4, CV_32F);
    R.copyTo(Tjw.rowRange(0, 3).colRange(0, 3));
    t.copyTo(Tjw.rowRange(0, 3).col(3));
    // 3.归一化坐标 ni, nj。
    vector<Point2f> ni, nj;
    ni.push_back(cam2nor(Point3d(-4, 2, 1)));
    nj.push_back(cam2nor(Point3d(2, 3, 1)));
    // 4.三角测量。
    Mat pw;
    cv::triangulatePoints(Tiw, Tjw, ni, nj, pw);
    pw = pw.rowRange(0, 3) / pw.at<float>(0, 3);
    cout << pw.t() << endl; // [-4, 2, 1]
}

对于像素坐标

// 相机坐标系到像素坐标系的变换。
cv::Point2f cam2pix(const cv::Point3f &p, const cv::Mat &K) {
    return cv::Point2f((p.x * K.at<float>(0, 0)) / p.z + K.at<float>(0, 2), (p.y * K.at<float>(1, 1)) / p.z + K.at<float>(1, 2));
}

int main() {
    // 1.相机 i 的内参 Ki 和位姿 Tiw。
    cv::Mat Ki = (cv::Mat_<float>(3, 3) << 521.0, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    cv::Mat Tiw = cv::Mat::eye(3, 4, CV_32F);
    // 2.相机 j 的内参 Kj 和位姿 Tjw。
    cv::Mat Kj = (cv::Mat_<float>(3, 3) << 521.0, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    cv::Mat R = (cv::Mat_<float>(3, 3) << 0, 1, 0, -1, 0, 0, 0, 0, 1);
    cv::Mat t = (cv::Mat_<float>(3, 1) << 0, -1, 0);
    cv::Mat Tjw(3, 4, CV_32F);
    R.copyTo(Tjw.rowRange(0, 3).colRange(0, 3));
    t.copyTo(Tjw.rowRange(0, 3).col(3));
    // 3.像素坐标匹配点。
    vector<Point2f> ptsi, ptsj;
    ptsi.push_back(cam2pix(Point3d(-4, 2, 1), Ki));
    ptsi.push_back(cam2pix(Point3d(1, 2, 3), Ki));
    ptsj.push_back(cam2pix(Point3d(2, 3, 1), Kj));
    ptsj.push_back(cam2pix(Point3d(2, -2, 3), Kj));
    // 4.三角测量。
    Mat pw;
    cv::triangulatePoints(Ki * Tiw, Kj * Tjw, ptsi, ptsj, pw);
    // 5.输出。
    for (size_t i = 0; i < pw.cols; i++) {
        Mat temp = pw.col(i).rowRange(0, 3) / pw.col(i).at<float>(3);
        cout << temp.t() << endl;
    }
}

7. 附录

---

7.1 方阵的特征分解

---

  设 A 是 n 阶方阵，如果数 $\lambda$ 和 n 维非零列向量 $\vec{x}$ 使关系式

$$A\vec{x}=\lambda \vec{x}$$

成立，那么，这样的数 $\lambda$ 称为矩阵 A 的特征值，非零向量 $\vec{x}$ 称为 A 的对应于特征值 $\lambda$ 的特征向量。

  相似矩阵定理：若 n 阶矩阵 A 与对角阵 $\Lambda =diag({\lambda }_1,{\lambda }_2,...,{\lambda }_n)$ 相似，即

$$A=P\Lambda P^{-1}$$

则 ${\lambda }_i$ 和 P 的第 i 列是方阵 A 的一对特征值和特征向量。

7.2 矩阵的奇异值分解

---

  特征分解仅适用于方阵，奇异值分解适用于任何矩阵，因此奇异值分解应用范围更广。

1. 奇异值分解与齐次线性方程组的解

  对于齐次线性方程组 Ax=0，对其系数矩阵 A 进行奇异值分解：$A=U\cdot \Lambda \cdot V^T$，则 $V^T$ 的最后一行（V 的最后一列）即是齐次线性方程组的一个特解。下面举例说明，假设有齐次线性方程组

$$\{\begin{array}{rl}{x}_1+x_2+x_3-1.5x_4& =0\\ x_1+x_2-2.33x_3+x_4& =0\\ x_1-4x_2+x_3+x_4& =0\\ -9x_1+x_2+x_3+x_4& =0\end{array}$$

可以使用奇异值分解得到一个特解：

int main() {
	Mat M = (Mat_<float>(4, 4) << 1, 1, 1, -1.5, 1, 1, -2.33, 1, 1, -4, 1, 1, -9, 1, 1, 1);
	Mat w, u, vt; // 奇异值、左奇异向量、右奇异向量的转置。
	cv::SVD::compute(M, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
	cout << vt.row(3) << endl; // [0.18256199, 0.36508748, 0.54812437, 0.73002881]。
}

2. 奇异值分解与特征分解

  矩阵 $A$、$A^{T}A$、$A{A}^T$ 的奇异值分解有如下关系：

$$\{\begin{array}{rl}A& =U\cdot \Lambda \cdot V^T\\ A{A}^T& =U\cdot \Lambda \cdot V^T\cdot (U\cdot \Lambda \cdot V^T{)}^T=U\cdot \Lambda \cdot V^T\cdot V\cdot {\Lambda }^T\cdot U^T=U\cdot \Lambda \cdot {\Lambda }^T\cdot U^T\\ A^{T}A& =(U\cdot \Lambda \cdot V^T{)}^T\cdot U\cdot \Lambda \cdot V^T=V\cdot {\Lambda }^T\cdot U^T\cdot U\cdot \Lambda \cdot V^T=V\cdot {\Lambda }^T\cdot \Lambda \cdot V^T\end{array}$$

由于 U 和 V 都是正交矩阵，所以 $U^T=U^{-1}$，$V^T=V^{-1}$。而且 $\Lambda \cdot {\Lambda }^T={\Lambda }^T\cdot \Lambda ={\Lambda }^2$ 仍是对角矩阵。所以

$$\{\begin{array}{rl}A{A}^T& =U\cdot {\Lambda }^2\cdot U^{-1}\\ A^{T}A& =V\cdot {\Lambda }^2\cdot V^{-1}\end{array}$$

可以看出，$A{A}^T$ 和 $A^{T}A$ 都与 ${\Lambda }^2$ 相似。所以 ${\lambda }_i^2$ 和 U 的第 i 列是 $A{A}^T$ 的一对特征值与特征向量，${\lambda }_i^2$ 和 V 的第 i 列是 $A^{T}A$ 的一对特征值与特征向量。事实上，所有对称方阵 A 都与对角方阵 $\Lambda$ 相似，即，对称矩阵可以分解为左右奇异矩阵互逆的形式 $A=P\Lambda P^{-1}$。

3. 使用 OpenCV 的特征分解和奇异值分解

int main() {
	cv::Mat A(3, 3, CV_32FC1);
	cv::RNG rng((unsigned int)time(NULL));
	rng.fill(A, cv::RNG::UNIFORM, 0.f, 10.f);

	cv::Mat eigen_values, eigen_vectors;
	cv::Mat W, U, VT;

	cout << "----------------- A = U W VT -----------------" << endl;
	cv::SVD::compute(A, W, U, VT, cv::SVD::FULL_UV);
	cout << U << endl << endl;
	cout << W << endl << endl;
	cout << VT << endl << endl;

	cout << "----------------- AAT x = lambda x -----------------" << endl;
	cv::Mat AAT = A * A.t();
	cv::eigen(AAT, eigen_values, eigen_vectors);
	cout << eigen_values << endl << endl;
	cout << eigen_vectors << endl << endl;

	cout << "----------------- AAT = U W UT -----------------" << endl;
	cv::SVD::compute(AAT, W, U, VT, cv::SVD::FULL_UV);
	cout << U << endl << endl;
	cout << W << endl << endl;
	cout << VT << endl << endl;

	cout << "----------------- ATA x = lambda x -----------------" << endl;
	cv::Mat ATA = A.t() * A;
	cv::eigen(ATA, eigen_values, eigen_vectors);
	cout << eigen_values << endl << endl;
	cout << eigen_vectors << endl << endl;

	cout << "----------------- ATA = V W VT -----------------" << endl;
	cv::SVD::compute(ATA, W, U, VT, cv::SVD::FULL_UV);
	cout << U << endl << endl;
	cout << W << endl << endl;
	cout << VT << endl << endl;

	cout << "----------------- ATA U.col(0) = lambda[0] U.col(0) -----------------" << endl;
	cv::Mat res = ATA * U.col(0) - W.at<float>(0, 0) * U.col(0);
	cout << res << endl;
}

  如代码的第 40 行，使用 OpenCV 的特征分解函数 eigen，得到的特征向量矩阵 eigen_vectors 的行向量是原矩阵的特征向量。

7.3 三阶对角矩阵的分解

---

  分解三阶对角矩阵 $\Lambda$：$diag({\lambda }_1,{\lambda }_2,{\lambda }_3)=d\cdot R+\vec{t}\cdot {\vec{n}}^T$。即，用等号前面的三个已知量表示等号后面的四个未知量。其中 $d$ 是标量，$R$ 是旋转矩阵，$\vec{t}$ 是向量，$\vec{n}$ 是单位向量，${\lambda }_1⩾{\lambda }_2⩾{\lambda }_3⩾0$。

  我们用 $[t_1,t_2,t_3{]}^T$ 表示 $\vec{t}$，用 $[n_1,n_2,n_3]$ 表示 $\vec{n}$，用 $\overrightarrow{e_1}$，$\overrightarrow{e_2}$，$\overrightarrow{e_3}$ 表示 $(1,0,0{)}^T$，$(0,1,0{)}^T$，$(0,0,1{)}^T$。原式可以拆分成三个等式

$$\{\begin{array}{c}d\cdot R\cdot \overrightarrow{e_1}+n_1\cdot \vec{t}={\lambda }_1\cdot \overrightarrow{e_1}\\ d\cdot R\cdot \overrightarrow{e_2}+n_2\cdot \vec{t}={\lambda }_2\cdot \overrightarrow{e_2}\\ d\cdot R\cdot \overrightarrow{e_3}+n_3\cdot \vec{t}={\lambda }_3\cdot \overrightarrow{e_3}\end{array}\text{\hspace{1em}(7.1)}$$

由公式 (7.1) 知

$$\begin{array}{rl}\vec{t}& =\\ & \frac{{\lambda }_1\cdot \overrightarrow{e_1}-d\cdot R\cdot \overrightarrow{e_1}}{n_1}=\frac{{\lambda }_2\cdot \overrightarrow{e_2}-d\cdot R\cdot \overrightarrow{e_2}}{n_2}=\frac{{\lambda }_3\cdot \overrightarrow{e_3}-d\cdot R\cdot \overrightarrow{e_3}}{n_3}\end{array}\text{\hspace{1em}(7.2)}$$

由公式 (7.2) 的第二行可以得到三个等式

$$\{\begin{array}{c}d\cdot R\cdot (n_1\cdot \overrightarrow{e_2}-n_2\cdot \overrightarrow{e_1})={\lambda }_2\cdot n_1\cdot \overrightarrow{e_2}-{\lambda }_1\cdot n_2\cdot \overrightarrow{e_1}\\ d\cdot R\cdot (n_1\cdot \overrightarrow{e_3}-n_3\cdot \overrightarrow{e_1})={\lambda }_3\cdot n_1\cdot \overrightarrow{e_3}-{\lambda }_1\cdot n_3\cdot \overrightarrow{e_1}\\ d\cdot R\cdot (n_2\cdot \overrightarrow{e_3}-n_3\cdot \overrightarrow{e_2})={\lambda }_3\cdot n_2\cdot \overrightarrow{e_3}-{\lambda }_2\cdot n_3\cdot \overrightarrow{e_2}\end{array}\text{\hspace{1em}(7.3)}$$

旋转矩阵 $R$ 具有保范性，即 $||R\cdot x|{|}_n=||x|{|}_n$。对公式 (7.3) 的第一个式子等号左边取二范数，有 $||d\cdot R\cdot (n_1\cdot \overrightarrow{e_2}-n_2\cdot \overrightarrow{e_1})|{|}_2=||d\cdot (n_1\cdot \overrightarrow{e_2}-n_2\cdot \overrightarrow{e_1})|{|}_2=||(-d\cdot n_2,d\cdot n_1,0)|{|}_2=\sqrt{d^2\cdot n_2^2+d^2\cdot n_1^2}$；对公式 (7.3) 的第一个式子等号右边取二范数，有 $||{\lambda }_2\cdot n_1\cdot \overrightarrow{e_2}-{\lambda }_1\cdot n_2\cdot \overrightarrow{e_1}|{|}_2=||(-{\lambda }_1\cdot n_2,{\lambda }_2\cdot n_1,0)|{|}_2=\sqrt{{\lambda }_1^2\cdot n_2^2+{\lambda }_2^2\cdot n_1^2}$。由 $\sqrt{d^2\cdot n_2^2+d^2\cdot n_1^2}=\sqrt{{\lambda }_1^2\cdot n_2^2+{\lambda }_2^2\cdot n_1^2}$ 得到公式 (7.4) 的第一个式子。同理，由公式 (7.3) 的后两个式子可以得到公式 (7.4) 的后两个式子：

$$\{\begin{array}{c}(d^2-{\lambda }_2^2)\cdot n_1^2+(d^2-{\lambda }_1^2)\cdot n_2^2=0\\ \\ (d^2-{\lambda }_3^2)\cdot n_1^2+(d^3-{\lambda }_1^3)\cdot n_3^2=0\\ \\ (d^2-{\lambda }_3^2)\cdot n_2^2+(d^3-{\lambda }_2^3)\cdot n_3^2=0\end{array}\text{\hspace{1em}(7.4)}$$

公式 (7.4) 是未知量为 $n_1^2$，$n_2^2$，$n_3^2$ 的齐次线性方程组。因为该齐次线性方程组有非零解，所以其系数矩阵的行列式为 0：

$$\left[\begin{array}{ccc}{d}^2-{\lambda }_2^2& d^2-{\lambda }_1^2& 0\\ & & \\ d^2-{\lambda }_3^2& 0& d^2-{\lambda }_1^2\\ & & \\ 0& d^2-{\lambda }_3^2& d^2-{\lambda }_2^2\end{array}\right]=-2(d^2-{\lambda }_1^2)(d^2-{\lambda }_2^2)(d^2-{\lambda }_3^2)=0\text{\hspace{1em}(7.5)}$$

  下面根据公式 (7.5) 等于 0，通过代回公式 (7.4)，分类讨论 $d$ 的取值：

1. $d^2={\lambda }_1^2$：则 $\{\begin{array}{cc}(d^2-{\lambda }_2^2)\cdot n_1^2& =0\\ (d^2-{\lambda }_3^2)\cdot n_1^2& =0\\ (d^2-{\lambda }_3^2)\cdot n_2^2+(d^2-{\lambda }_2^2)\cdot n_3^2& =0\end{array}$。
   1.1 $d^2={\lambda }_2^2$：则 $\{\begin{array}{cc}(d^2-{\lambda }_3^2)\cdot n_1^2& =0\\ (d^2-{\lambda }_3^2)\cdot n_2^2& =0\end{array}$。
     1.1.1 $d^2={\lambda }_3^2$：则 $\vec{n}$ 为任意向量。
     1.1.2 $d^2\ne {\lambda }_3^2$：则 $n_1^2=n_2^2=0$，$\vec{n}=(0,0,\pm 1)$。
   1.2 $d^2\ne {\lambda }_2^2$：则 $\{\begin{array}{cc}{n}_1^2& =0\\ (d^2-{\lambda }_3^2)\cdot n_2^2+(d^2-{\lambda }_2^2)\cdot n_3^2& =0\end{array}$，第二个式子两个括号内都是大于 0 的，所以该式不可能等于 0，这种情况不成立。

2. $d^2={\lambda }_2^2$：则 $\{\begin{array}{cc}(d^2-{\lambda }_1^2)\cdot n_2^2& =0\\ (d^2-{\lambda }_3^2)\cdot n_1^2+(d^2-{\lambda }_1^2)\cdot n_3^2& =0\\ (d^2-{\lambda }_3^2)\cdot n_2^2& =0\end{array}$。
   2.1 $d^2={\lambda }_1^2$：同 1.1。
   2.2 $d^2\ne {\lambda }_1^2$：则 $\{\begin{array}{cc}{n}_2^2& =0\\ (d^2-{\lambda }_3^2)\cdot n_1^2+(d^2-{\lambda }_1^2)\cdot n_3^2& =0\end{array}$，解得 $\{\begin{array}{rl}{n}_1^2& =({\lambda }_1^2-{\lambda }_2^2)/({\lambda }_1^2-{\lambda }_3^2)\\ n_2^2& =0\\ n_3^2& =({\lambda }_2^2-{\lambda }_3^2)/({\lambda }_1^2-{\lambda }_3^2)\end{array}$。

3. $d^2={\lambda }_3^2$：则 $\{\begin{array}{cc}(d^2-{\lambda }_2^2)\cdot n_1^2+(d^2-{\lambda }_1^2)\cdot n_2^2& =0\\ (d^2-{\lambda }_1^2)\cdot n_3^2& =0\\ (d^2-{\lambda }_2^2)\cdot n_3^2& =0\end{array}$。
   3.1 $d^2={\lambda }_2^2$：则 $\{\begin{array}{cc}(d^2-{\lambda }_1^2)\cdot n_2^2& =0\\ (d^2-{\lambda }_1^2)\cdot n_3^2& =0\end{array}$。
     3.1.1 $d^2={\lambda }_1^2$：同 1.1.1。
     3.1.2 $d^2\ne {\lambda }_1^2$：则 $n_2^2=n_3^2=0$，$\vec{n}=(\pm 1,0,0)$。
   3.2 $d^2\ne {\lambda }_2^2$：则 $\{\begin{array}{cc}(d^2-{\lambda }_2^2)\cdot n_1^2+(d^2-{\lambda }_1^2)\cdot n_2^2& =0\\ n_3^2& =0\end{array}$，第一个式子两个括号内都是小于 0 的，所以该式不可能等于 0，这种情况不成立。

综上所述，有

$$\{\begin{array}{rl}{d}^2={\lambda }_1^2={\lambda }_2^2={\lambda }_3^2,& \vec{n}是任意向量\\ d^2={\lambda }_1^2={\lambda }_2^2\ne {\lambda }_3^2,& \vec{n}=(0,0,\pm 1)\\ d^2={\lambda }_2^2\ne {\lambda }_1^2,& n_1^2=({\lambda }_1^2-{\lambda }_2^2)/({\lambda }_1^2-{\lambda }_3^2),n_2^2=0,n_3^2=({\lambda }_2^2-{\lambda }_3^2)/({\lambda }_1^2-{\lambda }_3^2)\\ d^2={\lambda }_3^2={\lambda }_2^2\ne {\lambda }_1^2,& \vec{n}=(\pm 1,0,0)\end{array}\text{\hspace{1em}(7.6)}$$

其中，公式 (7.6) 的第二个等式和第四个等式是第三个等式的特例，于是把第二、三、四个公式合并：

$$\{\begin{array}{rl}{d}^2={\lambda }_1^2={\lambda }_2^2={\lambda }_3^2,& \vec{n}是任意向量\\ d^2={\lambda }_2^2,& n_1^2=({\lambda }_1^2-{\lambda }_2^2)/({\lambda }_1^2-{\lambda }_3^2),n_2^2=0,n_3^2=({\lambda }_2^2-{\lambda }_3^2)/({\lambda }_1^2-{\lambda }_3^2)\end{array}\text{\hspace{1em}(7.7)}$$

至此，我们求得了单位向量 $\vec{n}$。我们着重考虑公式 (7.7) 的第二个等式，由于 $n_1$ 和 $n_3$ 的符号不能确定且 $n_2=0$，所以单位向量 $\vec{n}$ 有四个解。

  $d$ 可以取值 ${\lambda }_2$ 和 $-{\lambda }_2$：把 $d={\lambda }_2$ 和 $n_2=0$ 代回公式 (7.1) 的第二个等式，得 $R\cdot {\vec{e}}_2={\vec{e}}_2$，说明 $R$ 是绕 $\overrightarrow{e_2}$ 轴旋转的旋转矩阵；把 $d=-{\lambda }_2$ 和 $n_2=0$ 代回公式 (7.1) 的第二个等式，得 $R\cdot {\vec{e}}_2=-{\vec{e}}_2$，说明 $R$ 是绕 $-\overrightarrow{e_2}$ 轴旋转的旋转矩阵。据此，我们设

$$R={\left[\begin{array}{ccc}cos\theta & 0& -sin\theta \\ 0& 1& 0\\ sin\theta & 0& cos\theta \end{array}\right]}_{d=+{\lambda }_2}$$

$$R={\left[\begin{array}{ccc}cos\theta & 0& sin\theta \\ 0& -1& 0\\ sin\theta & 0& -cos\theta \end{array}\right]}_{d=-{\lambda }_2}$$

代回公式 (7.3) 的第二个等式，得

$$d=+{\lambda }_2\{\begin{array}{rl}sin\theta & =\frac{{\lambda }_1-{\lambda }_3}{{\lambda }_2}\cdot n_1\cdot n_3\\ & =\pm \frac{{\lambda }_1-{\lambda }_3}{{\lambda }_2}\cdot \frac{\sqrt{({\lambda }_1^2-{\lambda }_2^2)({\lambda }_2^2-{\lambda }_3^2)}}{{\lambda }_1^2-{\lambda }_3^2}=\pm \frac{\sqrt{({\lambda }_1^2-{\lambda }_2^2)({\lambda }_2^2-{\lambda }_3^2)}}{{\lambda }_2({\lambda }_1+{\lambda }_3)}\\ cos\theta & =\frac{{\lambda }_1\cdot n_3^2+{\lambda }_3\cdot n_1^2}{{\lambda }_2}=\frac{{\lambda }_2^2+{\lambda }_1\cdot {\lambda }_3}{{\lambda }_2({\lambda }_1+{\lambda }_3)}\end{array}\text{\hspace{1em}(7.8.1)}$$

$$d=-{\lambda }_2\{\begin{array}{rl}sin\theta & =\frac{{\lambda }_1+{\lambda }_3}{{\lambda }_2}\cdot n_1\cdot n_3\\ & =\pm \frac{{\lambda }_1+{\lambda }_3}{{\lambda }_2}\cdot \frac{\sqrt{({\lambda }_1^2-{\lambda }_2^2)({\lambda }_2^2-{\lambda }_3^2)}}{{\lambda }_1^2-{\lambda }_3^2}=\pm \frac{\sqrt{({\lambda }_1^2-{\lambda }_2^2)({\lambda }_2^2-{\lambda }_3^2)}}{{\lambda }_2({\lambda }_1-{\lambda }_3)}\\ cos\theta & =\frac{{\lambda }_3\cdot ({\lambda }_1^2-{\lambda }_2^2)-{\lambda }_1\cdot ({\lambda }_2^2-{\lambda }_3^2)}{{\lambda }_2\cdot ({\lambda }_1^2-{\lambda }_3^2)}=\frac{{\lambda }_1\cdot {\lambda }_3-{\lambda }_2^2}{{\lambda }_2({\lambda }_1-{\lambda }_3)}\end{array}\text{\hspace{1em}(7.8.2)}$$

由原式 $d\cdot R+\vec{t}\cdot {\vec{n}}^T=\Lambda$，知 $d\cdot R\cdot \vec{n}+\vec{t}=\Lambda \cdot \vec{n}$，所以

$$\begin{array}{rl}\vec{t}& =\Lambda \cdot \vec{n}-d\cdot R\cdot \vec{n}\\ & =\left[\begin{array}{ccc}{\lambda }_1& 0& 0\\ 0& {\lambda }_2& 0\\ 0& 0& {\lambda }_3\end{array}\right]\left[\begin{array}{c}{n}_1\\ 0\\ n_3\end{array}\right]-{\lambda }_2\left[\begin{array}{ccc}cos\theta & 0& -sin\theta \\ 0& 1& 0\\ sin\theta & 0& cos\theta \end{array}\right]\left[\begin{array}{c}{n}_1\\ 0\\ n_3\end{array}\right]\\ & =\left[\begin{array}{c}{\lambda }_1\cdot n_1-{\lambda }_2\cdot n_1\cdot cos\theta +{\lambda }_2\cdot n_3\cdot sin\theta \\ 0\\ {\lambda }_3\cdot n_3-{\lambda }_2\cdot n_1\cdot sin\theta -{\lambda }_2\cdot n_3\cdot cos\theta \end{array}\right]\\ & =\left[\begin{array}{c}{\lambda }_1\cdot n_1-{\lambda }_3\cdot n_1^3-{\lambda }_3\cdot n_1\cdot n_3^2\\ 0\\ {\lambda }_3\cdot n_3-{\lambda }_1\cdot n_1^2\cdot n_3-{\lambda }_1\cdot n_3^3\end{array}\right]\\ & =\left[\begin{array}{c}{\lambda }_1\cdot n_1-{\lambda }_3\cdot n_1(n_1^2+n_3^2)\\ 0\\ {\lambda }_3\cdot n_3-{\lambda }_1\cdot n_3(n_1^2+n_3^2)\end{array}\right]=\left[\begin{array}{c}{\lambda }_1\cdot n_1-{\lambda }_3\cdot n_1\\ 0\\ {\lambda }_3\cdot n_3-{\lambda }_1\cdot n_3\end{array}\right]\\ & =({\lambda }_1-{\lambda }_3){\left[\begin{array}{c}{n}_1\\ 0\\ -n_3\end{array}\right]}_{d=+{\lambda }_2}\end{array}\text{\hspace{1em}(7.9.1)}$$

$$\begin{array}{rl}\vec{t}& =\Lambda \cdot \vec{n}-d\cdot R\cdot \vec{n}\\ & =\left[\begin{array}{ccc}{\lambda }_1& 0& 0\\ 0& {\lambda }_2& 0\\ 0& 0& {\lambda }_3\end{array}\right]\left[\begin{array}{c}{n}_1\\ 0\\ n_3\end{array}\right]+{\lambda }_2\left[\begin{array}{ccc}cos\theta & 0& sin\theta \\ 0& -1& 0\\ sin\theta & 0& -cos\theta \end{array}\right]\left[\begin{array}{c}{n}_1\\ 0\\ n_3\end{array}\right]\\ & =\left[\begin{array}{c}{\lambda }_1\cdot n_1+{\lambda }_2\cdot (n_1\cdot cos\theta +n_3\cdot sin\theta )\\ 0\\ {\lambda }_3\cdot n_3+{\lambda }_2\cdot (n_1\cdot sin\theta -n_3\cdot cos\theta )\end{array}\right]\\ & =diag(n_1,0,n_3)\cdot \left[\begin{array}{c}{\lambda }_1+\frac{{\lambda }_1\cdot {\lambda }_3-{\lambda }_2^2}{{\lambda }_1-{\lambda }_3}+({\lambda }_1+{\lambda }_3)\cdot n_3^2\\ 0\\ {\lambda }_3+({\lambda }_1+{\lambda }_3)\cdot n_1^2-\frac{{\lambda }_1\cdot {\lambda }_3-{\lambda }_2^2}{{\lambda }_1-{\lambda }_3}\end{array}\right]\\ & =diag(n_1,0,n_3)\cdot \left[\begin{array}{c}{\lambda }_1+{\lambda }_3\\ 0\\ {\lambda }_1+{\lambda }_3\end{array}\right]\\ & =({\lambda }_1+{\lambda }_3){\left[\begin{array}{c}{n}_1\\ 0\\ n_3\end{array}\right]}_{d=-{\lambda }_2}\end{array}\text{\hspace{1em}(7.9.2)}$$

  根据公式 (7.7)、(7.8) 、(7.9) 可知一共得到八组解：$d={\lambda }_2$ 时，$n_1$ 和 $n_3$ 有四种符号组合，可以解得四组 $(\vec{n},d,R,\vec{t})$；$d=-{\lambda }_2$ 时，$n_1$ 和 $n_3$ 有四种符号组合，可以解得四组 $(\vec{n},d,R,\vec{t})$。

8. 参考

---

1. 求基础矩阵和单应矩阵前的归一化，CSDN。
2. 求基础矩阵和单应矩阵前的归一化，博客园。
3. 三角测量算法，CSDN。
4. 分解单应矩阵，CSDN。
5. 分解单应矩阵，知乎专栏。
6. 分解单应矩阵，ResearchGate。
7. 特征值分解与奇异值分解，知乎专栏。
8. 奇异值分解，CSDN。
9. 绕坐标轴旋转的旋转矩阵，CSDN。
10. 绕坐标轴旋转的旋转矩阵，博客园。