欢迎访问我的博客首页。
6. 三角测量
6.1 使用归一化坐标
假设世界坐标系 w 中的坐标到相机坐标系 i,j 的坐标的变换分别是 T i w {\bf T}_{iw} Tiw, T j w {\bf T}_{jw} Tjw。已知世界坐标系中的一点 p w {\bf p}_w pw 在相机坐标系 i,j 中的坐标分别是 p i = s i [ u i , v i , 1 ] T {\bf p}_i = s_i [u_i, v_i, 1]^T pi=si[ui,vi,1]T, p j = s j [ u j , v j , 1 ] T {\bf p}_j = s_j [u_j, v_j, 1]^T pj=sj[uj,vj,1]T。求 p w {\bf p}_w pw。
由题设知
{ s i [ u i v i 1 ] = T i w p w s j [ u j v j 1 ] = T j w p w (6.1) \left\{\begin{aligned} s_i \left[ \begin{matrix} u_i \\ v_i \\ 1 \end{matrix} \right] &= {\bf T}_{iw} {\bf p}_w \\ s_j \left[ \begin{matrix} u_j \\ v_j \\ 1 \end{matrix} \right] &= {\bf T}_{jw} {\bf p}_w \end{aligned}\right. \tag{6.1} ⎩ ⎨ ⎧si uivi1 sj ujvj1 =Tiwpw=Tjwpw(6.1)
写成方程组的形式为
{ s i u i = T i w . r o w ( 0 ) ⋅ p w s i v i = T i w . r o w ( 1 ) ⋅ p w s i = T i w . r o w ( 2 ) ⋅ p w s j u j = T j w . r o w ( 0 ) ⋅ p w s j v j = T j w . r o w ( 1 ) ⋅ p w s j = T j w . r o w ( 2 ) ⋅ p w \left\{\begin{aligned} s_i u_i &= {\bf T}_{iw}.row(0) \cdot {\bf p}_w \\ s_i v_i &= {\bf T}_{iw}.row(1) \cdot {\bf p}_w \\ s_i &= {\bf T}_{iw}.row(2) \cdot {\bf p}_w \\ s_j u_j &= {\bf T}_{jw}.row(0) \cdot {\bf p}_w \\ s_j v_j &= {\bf T}_{jw}.row(1) \cdot {\bf p}_w \\ s_j &= {\bf T}_{jw}.row(2) \cdot {\bf p}_w \end{aligned}\right. ⎩ ⎨ ⎧siuisivisisjujsjvjsj=Tiw.row(0)⋅pw=Tiw.row(1)⋅pw=Tiw.row(2)⋅pw=Tjw.row(0)⋅pw=Tjw.row(1)⋅pw=Tjw.row(2)⋅pw
把第 3 个等式代入第 1,2 个等式,把第 6 个等式代入第 4,5 个等式得
{ [ T i w . r o w ( 2 ) ⋅ u i − T i w . r o w ( 0 ) ] ⋅ p w = 0 [ T i w . r o w ( 2 ) ⋅ v i − T i w . r o w ( 1 ) ] ⋅ p w = 0 [ T j w . r o w ( 2 ) ⋅ u j − T j w . r o w ( 0 ) ] ⋅ p w = 0 [ T j w . r o w ( 2 ) ⋅ v j − T j w . r o w ( 1 ) ] ⋅ p w = 0 \left\{\begin{aligned} [{\bf T}_{iw}.row(2) \cdot u_i - {\bf T}_{iw}.row(0)] \cdot {\bf p}_w &= 0 \\ [{\bf T}_{iw}.row(2) \cdot v_i - {\bf T}_{iw}.row(1)] \cdot {\bf p}_w &= 0 \\ [{\bf T}_{jw}.row(2) \cdot u_j - {\bf T}_{jw}.row(0)] \cdot {\bf p}_w &= 0 \\ [{\bf T}_{jw}.row(2) \cdot v_j - {\bf T}_{jw}.row(1)] \cdot {\bf p}_w &= 0 \end{aligned}\right. ⎩ ⎨ ⎧[Tiw.row(2)⋅ui−Tiw.row(0)]⋅pw[Tiw.row(2)⋅vi−Tiw.row(1)]⋅pw[Tjw.row(2)⋅uj−Tjw.row(0)]⋅pw[Tjw.row(2)⋅vj−Tjw.row(1)]⋅pw=0=0=0=0
求解该齐次线性方程组即可得到 p w {\bf p}_w pw。一种方法是对其系数矩阵 A 奇异值分解 A = U Λ V T \bf A = U \Lambda V^T A=UΛVT,则矩阵 V \bf V 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 维非零列向量 x ⃗ \vec x x 使关系式
A x ⃗ = λ x ⃗ A \vec x = \lambda \vec x Ax=λx
成立,那么,这样的数 λ \lambda λ 称为矩阵 A 的特征值,非零向量 x ⃗ \vec x x 称为 A 的对应于特征值 λ \lambda λ 的特征向量。
相似矩阵定理:若 n 阶矩阵 A 与对角阵 Λ = d i a g ( λ 1 , λ 2 , . . . , λ n ) \Lambda = diag(\lambda_1, \lambda_2, ..., \lambda_n) Λ=diag(λ1,λ2,...,λn) 相似,即
A = P Λ P − 1 A = P \Lambda P^{-1} A=PΛP−1
则 λ i \lambda_i λi 和 P 的第 i 列是方阵 A 的一对特征值和特征向量。
7.2 矩阵的奇异值分解
特征分解仅适用于方阵,奇异值分解适用于任何矩阵,因此奇异值分解应用范围更广。
- 奇异值分解与齐次线性方程组的解
对于齐次线性方程组 Ax=0,对其系数矩阵 A 进行奇异值分解: A = U ⋅ Λ ⋅ V T A = U \cdot \Lambda \cdot V^T A=U⋅Λ⋅VT,则 V T V^T VT 的最后一行(V 的最后一列)即是齐次线性方程组的一个特解。下面举例说明,假设有齐次线性方程组
{ x 1 + x 2 + x 3 − 1.5 x 4 = 0 x 1 + x 2 − 2.33 x 3 + x 4 = 0 x 1 − 4 x 2 + x 3 + x 4 = 0 − 9 x 1 + x 2 + x 3 + x 4 = 0 \left\{ \begin{aligned} x_1 + x_2 + x_3 - 1.5 x_4 &= 0 \\ x_1 + x_2 - 2.33 x_3 + x_4 &= 0 \\ x_1 - 4 x_2 + x_3 + x_4 &= 0 \\ -9 x_1 + x_2 + x_3 + x_4 &= 0 \end{aligned} \right. ⎩ ⎨ ⎧x1+x2+x3−1.5x4x1+x2−2.33x3+x4x1−4x2+x3+x4−9x1+x2+x3+x4=0=0=0=0
可以使用奇异值分解得到一个特解:
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]。
}
- 奇异值分解与特征分解
矩阵 A A A、 A T A A^TA ATA、 A A T AA^T AAT 的奇异值分解有如下关系:
{ A = U ⋅ Λ ⋅ V T A A T = U ⋅ Λ ⋅ V T ⋅ ( U ⋅ Λ ⋅ V T ) T = U ⋅ Λ ⋅ V T ⋅ V ⋅ Λ T ⋅ U T = U ⋅ Λ ⋅ Λ T ⋅ U T A T A = ( U ⋅ Λ ⋅ V T ) T ⋅ U ⋅ Λ ⋅ V T = V ⋅ Λ T ⋅ U T ⋅ U ⋅ Λ ⋅ V T = V ⋅ Λ T ⋅ Λ ⋅ V T \left\{ \begin{aligned} A &= U \cdot \Lambda \cdot V^T \\ AA^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^TA &= (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{aligned} \right. ⎩ ⎨ ⎧AAATATA=U⋅Λ⋅VT=U⋅Λ⋅VT⋅(U⋅Λ⋅VT)T=U⋅Λ⋅VT⋅V⋅ΛT⋅UT=U⋅Λ⋅ΛT⋅UT=(U⋅Λ⋅VT)T⋅U⋅Λ⋅VT=V⋅ΛT⋅UT⋅U⋅Λ⋅VT=V⋅ΛT⋅Λ⋅VT
由于 U 和 V 都是正交矩阵,所以 U T = U − 1 U^T = U^{-1} UT=U−1, V T = V − 1 V^T = V^{-1} VT=V−1。而且 Λ ⋅ Λ T = Λ T ⋅ Λ = Λ 2 \Lambda \cdot \Lambda^T = \Lambda^T \cdot \Lambda = \Lambda^2 Λ⋅ΛT=ΛT⋅Λ=Λ2 仍是对角矩阵。所以
{ A A T = U ⋅ Λ 2 ⋅ U − 1 A T A = V ⋅ Λ 2 ⋅ V − 1 \left\{ \begin{aligned} AA^T &= U \cdot \Lambda^2 \cdot U^{-1} \\ A^TA &= V \cdot \Lambda^2 \cdot V^{-1} \end{aligned} \right. {AATATA=U⋅Λ2⋅U−1=V⋅Λ2⋅V−1
可以看出, A A T AA^T AAT 和 A T A A^TA ATA 都与 Λ 2 \Lambda^2 Λ2 相似。所以 λ i 2 \lambda_i^2 λi2 和 U 的第 i 列是 A A T AA^T AAT 的一对特征值与特征向量, λ i 2 \lambda_i^2 λi2 和 V 的第 i 列是 A T A A^TA ATA 的一对特征值与特征向量。事实上,所有对称方阵 A 都与对角方阵 Λ \Lambda Λ 相似,即,对称矩阵可以分解为左右奇异矩阵互逆的形式 A = P Λ P − 1 A = P \Lambda P^{-1} A=PΛP−1。
- 使用 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 Λ: d i a g ( λ 1 , λ 2 , λ 3 ) = d ⋅ R + t ⃗ ⋅ n ⃗ T diag(\lambda_1, \lambda_2, \lambda_3) = d \cdot R + \vec t \cdot \vec n^T diag(λ1,λ2,λ3)=d⋅R+t⋅nT。即,用等号前面的三个已知量表示等号后面的四个未知量。其中 d d d 是标量, R R R 是旋转矩阵, t ⃗ \vec t t 是向量, n ⃗ \vec n n 是单位向量, λ 1 ⩾ λ 2 ⩾ λ 3 ⩾ 0 \lambda_1 \geqslant \lambda_2 \geqslant \lambda_3 \geqslant 0 λ1⩾λ2⩾λ3⩾0。
我们用 [ t 1 , t 2 , t 3 ] T [t_1, t_2, t_3]^T [t1,t2,t3]T 表示 t ⃗ \vec t t,用 [ n 1 , n 2 , n 3 ] [n_1, n_2, n_3] [n1,n2,n3] 表示 n ⃗ \vec n n,用 e 1 ⃗ \vec {e_1} e1, e 2 ⃗ \vec {e_2} e2, e 3 ⃗ \vec {e_3} e3 表示 ( 1 , 0 , 0 ) T (1, 0, 0)^T (1,0,0)T, ( 0 , 1 , 0 ) T (0, 1, 0)^T (0,1,0)T, ( 0 , 0 , 1 ) T (0, 0, 1)^T (0,0,1)T。原式可以拆分成三个等式
{ d ⋅ R ⋅ e 1 ⃗ + n 1 ⋅ t ⃗ = λ 1 ⋅ e 1 ⃗ d ⋅ R ⋅ e 2 ⃗ + n 2 ⋅ t ⃗ = λ 2 ⋅ e 2 ⃗ d ⋅ R ⋅ e 3 ⃗ + n 3 ⋅ t ⃗ = λ 3 ⋅ e 3 ⃗ (7.1) \left\{ \begin{matrix} d \cdot R \cdot \vec {e_1} + n_1 \cdot \vec t = \lambda_1 \cdot \vec {e_1} \\ d \cdot R \cdot \vec {e_2} + n_2 \cdot \vec t = \lambda_2 \cdot \vec {e_2} \\ d \cdot R \cdot \vec {e_3} + n_3 \cdot \vec t = \lambda_3 \cdot \vec {e_3} \end{matrix} \right. \tag{7.1} ⎩ ⎨ ⎧d⋅R⋅e1+n1⋅t=λ1⋅e1d⋅R⋅e2+n2⋅t=λ2⋅e2d⋅R⋅e3+n3⋅t=λ3⋅e3(7.1)
由公式 (7.1) 知
t ⃗ = λ 1 ⋅ e 1 ⃗ − d ⋅ R ⋅ e 1 ⃗ n 1 = λ 2 ⋅ e 2 ⃗ − d ⋅ R ⋅ e 2 ⃗ n 2 = λ 3 ⋅ e 3 ⃗ − d ⋅ R ⋅ e 3 ⃗ n 3 (7.2) \begin{aligned} \vec t &= \\ & \frac{\lambda_1 \cdot \vec{e_1} - d \cdot R \cdot \vec{e_1}}{n_1} = \frac{\lambda_2 \cdot \vec{e_2} - d \cdot R \cdot \vec{e_2}}{n_2} = \frac{\lambda_3 \cdot \vec{e_3} - d \cdot R \cdot \vec{e_3}}{n_3} \end{aligned} \tag{7.2} t=n1λ1⋅e1−d⋅R⋅e1=n2λ2⋅e2−d⋅R⋅e2=n3λ3⋅e3−d⋅R⋅e3(7.2)
由公式 (7.2) 的第二行可以得到三个等式
{ d ⋅ R ⋅ ( n 1 ⋅ e 2 ⃗ − n 2 ⋅ e 1 ⃗ ) = λ 2 ⋅ n 1 ⋅ e 2 ⃗ − λ 1 ⋅ n 2 ⋅ e 1 ⃗ d ⋅ R ⋅ ( n 1 ⋅ e 3 ⃗ − n 3 ⋅ e 1 ⃗ ) = λ 3 ⋅ n 1 ⋅ e 3 ⃗ − λ 1 ⋅ n 3 ⋅ e 1 ⃗ d ⋅ R ⋅ ( n 2 ⋅ e 3 ⃗ − n 3 ⋅ e 2 ⃗ ) = λ 3 ⋅ n 2 ⋅ e 3 ⃗ − λ 2 ⋅ n 3 ⋅ e 2 ⃗ (7.3) \left\{ \begin{matrix} d \cdot R \cdot (n_1 \cdot \vec{e_2} - n_2 \cdot \vec{e_1}) = \lambda_2 \cdot n_1 \cdot \vec{e_2} - \lambda_1 \cdot n_2 \cdot \vec{e_1} \\ d \cdot R \cdot (n_1 \cdot \vec{e_3} - n_3 \cdot \vec{e_1}) = \lambda_3 \cdot n_1 \cdot \vec{e_3} - \lambda_1 \cdot n_3 \cdot \vec{e_1} \\ d \cdot R \cdot (n_2 \cdot \vec{e_3} - n_3 \cdot \vec{e_2}) = \lambda_3 \cdot n_2 \cdot \vec{e_3} - \lambda_2 \cdot n_3 \cdot \vec{e_2} \end{matrix} \right. \tag{7.3} ⎩ ⎨ ⎧d⋅R⋅(n1⋅e2−n2⋅e1)=λ2⋅n1⋅e2−λ1⋅n2⋅e1d⋅R⋅(n1⋅e3−n3⋅e1)=λ3⋅n1⋅e3−λ1⋅n3⋅e1d⋅R⋅(n2⋅e3−n3⋅e2)=λ3⋅n2⋅e3−λ2⋅n3⋅e2(7.3)
旋转矩阵 R R R 具有保范性,即 ∣ ∣ R ⋅ x ∣ ∣ n = ∣ ∣ x ∣ ∣ n ||R \cdot x||_n = ||x||_n ∣∣R⋅x∣∣n=∣∣x∣∣n。对公式 (7.3) 的第一个式子等号左边取二范数,有 ∣ ∣ d ⋅ R ⋅ ( n 1 ⋅ e 2 ⃗ − n 2 ⋅ e 1 ⃗ ) ∣ ∣ 2 = ∣ ∣ d ⋅ ( n 1 ⋅ e 2 ⃗ − n 2 ⋅ e 1 ⃗ ) ∣ ∣ 2 = ∣ ∣ ( − d ⋅ n 2 , d ⋅ n 1 , 0 ) ∣ ∣ 2 = d 2 ⋅ n 2 2 + d 2 ⋅ n 1 2 ||d \cdot R \cdot (n_1 \cdot \vec{e_2} - n_2 \cdot \vec{e_1})||_2 = ||d \cdot (n_1 \cdot \vec{e_2} - n_2 \cdot \vec{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} ∣∣d⋅R⋅(n1⋅e2−n2⋅e1)∣∣2=∣∣d⋅(n1⋅e2−n2⋅e1)∣∣2=∣∣(−d⋅n2,d⋅n1,0)∣∣2=d2⋅n22+d2⋅n12;对公式 (7.3) 的第一个式子等号右边取二范数,有 ∣ ∣ λ 2 ⋅ n 1 ⋅ e 2 ⃗ − λ 1 ⋅ n 2 ⋅ e 1 ⃗ ∣ ∣ 2 = ∣ ∣ ( − λ 1 ⋅ n 2 , λ 2 ⋅ n 1 , 0 ) ∣ ∣ 2 = λ 1 2 ⋅ n 2 2 + λ 2 2 ⋅ n 1 2 ||\lambda_2 \cdot n_1 \cdot \vec{e_2} - \lambda_1 \cdot n_2 \cdot \vec{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} ∣∣λ2⋅n1⋅e2−λ1⋅n2⋅e1∣∣2=∣∣(−λ1⋅n2,λ2⋅n1,0)∣∣2=λ12⋅n22+λ22⋅n12。由 d 2 ⋅ n 2 2 + d 2 ⋅ n 1 2 = λ 1 2 ⋅ n 2 2 + λ 2 2 ⋅ 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} d2⋅n22+d2⋅n12=λ12⋅n22+λ22⋅n12 得到公式 (7.4) 的第一个式子。同理,由公式 (7.3) 的后两个式子可以得到公式 (7.4) 的后两个式子:
{ ( d 2 − λ 2 2 ) ⋅ n 1 2 + ( d 2 − λ 1 2 ) ⋅ n 2 2 = 0 ( d 2 − λ 3 2 ) ⋅ n 1 2 + ( d 3 − λ 1 3 ) ⋅ n 3 2 = 0 ( d 2 − λ 3 2 ) ⋅ n 2 2 + ( d 3 − λ 2 3 ) ⋅ n 3 2 = 0 (7.4) \left\{ \begin{matrix} (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{matrix} \right. \tag{7.4} ⎩ ⎨ ⎧(d2−λ22)⋅n12+(d2−λ12)⋅n22=0(d2−λ32)⋅n12+(d3−λ13)⋅n32=0(d2−λ32)⋅n22+(d3−λ23)⋅n32=0(7.4)
公式 (7.4) 是未知量为 n 1 2 n_1^2 n12, n 2 2 n_2^2 n22, n 3 2 n_3^2 n32 的齐次线性方程组。因为该齐次线性方程组有非零解,所以其系数矩阵的行列式为 0:
[ d 2 − λ 2 2 d 2 − λ 1 2 0 d 2 − λ 3 2 0 d 2 − λ 1 2 0 d 2 − λ 3 2 d 2 − λ 2 2 ] = − 2 ( d 2 − λ 1 2 ) ( d 2 − λ 2 2 ) ( d 2 − λ 3 2 ) = 0 (7.5) \begin{bmatrix} 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{bmatrix} = -2 (d^2 - \lambda_1^2) (d^2 - \lambda_2^2) (d^2 - \lambda_3^2) = 0 \tag{7.5} d2−λ22d2−λ320d2−λ120d2−λ320d2−λ12d2−λ22 =−2(d2−λ12)(d2−λ22)(d2−λ32)=0(7.5)
下面根据公式 (7.5) 等于 0,通过代回公式 (7.4),分类讨论 d d d 的取值:
-
d 2 = λ 1 2 d^2 = \lambda_1^2 d2=λ12:则 { ( d 2 − λ 2 2 ) ⋅ n 1 2 = 0 ( d 2 − λ 3 2 ) ⋅ n 1 2 = 0 ( d 2 − λ 3 2 ) ⋅ n 2 2 + ( d 2 − λ 2 2 ) ⋅ n 3 2 = 0 \left\{\begin{matrix} (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{matrix}\right. ⎩ ⎨ ⎧(d2−λ22)⋅n12(d2−λ32)⋅n12(d2−λ32)⋅n22+(d2−λ22)⋅n32=0=0=0。
1.1 d 2 = λ 2 2 d^2 = \lambda_2^2 d2=λ22:则 { ( d 2 − λ 3 2 ) ⋅ n 1 2 = 0 ( d 2 − λ 3 2 ) ⋅ n 2 2 = 0 \left\{\begin{matrix} (d^2 - \lambda_3^2) \cdot n_1^2 &= 0 \\ (d^2 - \lambda_3^2) \cdot n_2^2 &= 0 \end{matrix}\right. {(d2−λ32)⋅n12(d2−λ32)⋅n22=0=0。
1.1.1 d 2 = λ 3 2 d^2 = \lambda_3^2 d2=λ32:则 n ⃗ \vec n n 为任意向量。
1.1.2 d 2 ≠ λ 3 2 d^2 \neq \lambda_3^2 d2=λ32:则 n 1 2 = n 2 2 = 0 n_1^2 = n_2^2 = 0 n12=n22=0, n ⃗ = ( 0 , 0 , ± 1 ) \vec n = (0, 0, \pm 1) n=(0,0,±1)。
1.2 d 2 ≠ λ 2 2 d^2 \neq \lambda_2^2 d2=λ22:则 { n 1 2 = 0 ( d 2 − λ 3 2 ) ⋅ n 2 2 + ( d 2 − λ 2 2 ) ⋅ n 3 2 = 0 \left\{\begin{matrix} n_1^2 &= 0 \\ (d^2 - \lambda_3^2) \cdot n_2^2 + (d^2 - \lambda_2^2) \cdot n_3^2 &= 0 \end{matrix}\right. {n12(d2−λ32)⋅n22+(d2−λ22)⋅n32=0=0,第二个式子两个括号内都是大于 0 的,所以该式不可能等于 0,这种情况不成立。 -
d 2 = λ 2 2 d^2 = \lambda_2^2 d2=λ22:则 { ( d 2 − λ 1 2 ) ⋅ n 2 2 = 0 ( d 2 − λ 3 2 ) ⋅ n 1 2 + ( d 2 − λ 1 2 ) ⋅ n 3 2 = 0 ( d 2 − λ 3 2 ) ⋅ n 2 2 = 0 \left\{\begin{matrix} (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{matrix}\right. ⎩ ⎨ ⎧(d2−λ12)⋅n22(d2−λ32)⋅n12+(d2−λ12)⋅n32(d2−λ32)⋅n22=0=0=0。
2.1 d 2 = λ 1 2 d^2 = \lambda_1^2 d2=λ12:同 1.1。
2.2 d 2 ≠ λ 1 2 d^2 \neq \lambda_1^2 d2=λ12:则 { n 2 2 = 0 ( d 2 − λ 3 2 ) ⋅ n 1 2 + ( d 2 − λ 1 2 ) ⋅ n 3 2 = 0 \left\{\begin{matrix} n_2^2 &= 0 \\ (d^2 - \lambda_3^2) \cdot n_1^2 + (d^2 - \lambda_1^2) \cdot n_3^2 &= 0 \end{matrix}\right. {n22(d2−λ32)⋅n12+(d2−λ12)⋅n32=0=0,解得 { n 1 2 = ( λ 1 2 − λ 2 2 ) / ( λ 1 2 − λ 3 2 ) n 2 2 = 0 n 3 2 = ( λ 2 2 − λ 3 2 ) / ( λ 1 2 − λ 3 2 ) \left\{\begin{aligned} 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{aligned}\right. ⎩ ⎨ ⎧n12n22n32=(λ12−λ22)/(λ12−λ32)=0=(λ22−λ32)/(λ12−λ32)。 -
d 2 = λ 3 2 d^2 = \lambda_3^2 d2=λ32:则 { ( d 2 − λ 2 2 ) ⋅ n 1 2 + ( d 2 − λ 1 2 ) ⋅ n 2 2 = 0 ( d 2 − λ 1 2 ) ⋅ n 3 2 = 0 ( d 2 − λ 2 2 ) ⋅ n 3 2 = 0 \left\{\begin{matrix} (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{matrix}\right. ⎩ ⎨ ⎧(d2−λ22)⋅n12+(d2−λ12)⋅n22(d2−λ12)⋅n32(d2−λ22)⋅n32=0=0=0。
3.1 d 2 = λ 2 2 d^2 = \lambda_2^2 d2=λ22:则 { ( d 2 − λ 1 2 ) ⋅ n 2 2 = 0 ( d 2 − λ 1 2 ) ⋅ n 3 2 = 0 \left\{\begin{matrix} (d^2 - \lambda_1^2) \cdot n_2^2 &= 0 \\ (d^2 - \lambda_1^2) \cdot n_3^2 &= 0 \end{matrix}\right. {(d2−λ12)⋅n22(d2−λ12)⋅n32=0=0。
3.1.1 d 2 = λ 1 2 d^2 = \lambda_1^2 d2=λ12:同 1.1.1。
3.1.2 d 2 ≠ λ 1 2 d^2 \neq \lambda_1^2 d2=λ12:则 n 2 2 = n 3 2 = 0 n_2^2 = n_3^2 = 0 n22=n32=0, n ⃗ = ( ± 1 , 0 , 0 ) \vec n = (\pm 1, 0, 0) n=(±1,0,0)。
3.2 d 2 ≠ λ 2 2 d^2 \neq \lambda_2^2 d2=λ22:则 { ( d 2 − λ 2 2 ) ⋅ n 1 2 + ( d 2 − λ 1 2 ) ⋅ n 2 2 = 0 n 3 2 = 0 \left\{\begin{matrix} (d^2 - \lambda_2^2) \cdot n_1^2 + (d^2 - \lambda_1^2) \cdot n_2^2 &= 0 \\ n_3^2 &= 0 \end{matrix}\right. {(d2−λ22)⋅n12+(d2−λ12)⋅n22n32=0=0,第一个式子两个括号内都是小于 0 的,所以该式不可能等于 0,这种情况不成立。
综上所述,有
{ d 2 = λ 1 2 = λ 2 2 = λ 3 2 , n ⃗ 是任意向量 d 2 = λ 1 2 = λ 2 2 ≠ λ 3 2 , n ⃗ = ( 0 , 0 , ± 1 ) d 2 = λ 2 2 ≠ λ 1 2 , n 1 2 = ( λ 1 2 − λ 2 2 ) / ( λ 1 2 − λ 3 2 ) , n 2 2 = 0 , n 3 2 = ( λ 2 2 − λ 3 2 ) / ( λ 1 2 − λ 3 2 ) d 2 = λ 3 2 = λ 2 2 ≠ λ 1 2 , n ⃗ = ( ± 1 , 0 , 0 ) (7.6) \left\{\begin{aligned} d^2 = \lambda_1^2 = \lambda_2^2 = \lambda_3^2, \quad\quad &\vec n 是任意向量 \\ d^2 = \lambda_1^2 = \lambda_2^2 \neq \lambda_3^2, \quad\quad &\vec n = (0, 0, \pm 1) \\ d^2 = \lambda_2^2 \neq \lambda_1^2, \quad\quad &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 \neq \lambda_1^2, \quad\quad &\vec n = (\pm 1, 0, 0) \end{aligned}\right. \tag{7.6} ⎩ ⎨ ⎧d2=λ12=λ22=λ32,d2=λ12=λ22=λ32,d2=λ22=λ12,d2=λ32=λ22=λ12,n是任意向量n=(0,0,±1)n12=(λ12−λ22)/(λ12−λ32),n22=0,n32=(λ22−λ32)/(λ12−λ32)n=(±1,0,0)(7.6)
其中,公式 (7.6) 的第二个等式和第四个等式是第三个等式的特例,于是把第二、三、四个公式合并:
{ d 2 = λ 1 2 = λ 2 2 = λ 3 2 , n ⃗ 是任意向量 d 2 = λ 2 2 , n 1 2 = ( λ 1 2 − λ 2 2 ) / ( λ 1 2 − λ 3 2 ) , n 2 2 = 0 , n 3 2 = ( λ 2 2 − λ 3 2 ) / ( λ 1 2 − λ 3 2 ) (7.7) \left\{\begin{aligned} d^2 = \lambda_1^2 = \lambda_2^2 = \lambda_3^2, \quad\quad &\vec n 是任意向量 \\ d^2 = \lambda_2^2, \quad\quad &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{aligned}\right. \tag{7.7} {d2=λ12=λ22=λ32,d2=λ22,n是任意向量n12=(λ12−λ22)/(λ12−λ32),n22=0,n32=(λ22−λ32)/(λ12−λ32)(7.7)
至此,我们求得了单位向量 n ⃗ \vec n n。我们着重考虑公式 (7.7) 的第二个等式,由于 n 1 n_1 n1 和 n 3 n_3 n3 的符号不能确定且 n 2 = 0 n_2 = 0 n2=0,所以单位向量 n ⃗ \vec n n 有四个解。
d d d 可以取值 λ 2 \lambda_2 λ2 和 − λ 2 -\lambda_2 −λ2:把 d = λ 2 d = \lambda_2 d=λ2 和 n 2 = 0 n_2 = 0 n2=0 代回公式 (7.1) 的第二个等式,得 R ⋅ e ⃗ 2 = e ⃗ 2 R \cdot \vec e_2 = \vec e_2 R⋅e2=e2,说明 R R R 是绕 e 2 ⃗ \vec {e_2} e2 轴旋转的旋转矩阵;把 d = − λ 2 d = - \lambda_2 d=−λ2 和 n 2 = 0 n_2 = 0 n2=0 代回公式 (7.1) 的第二个等式,得 R ⋅ e ⃗ 2 = − e ⃗ 2 R \cdot \vec e_2 = - \vec e_2 R⋅e2=−e2,说明 R R R 是绕 − e 2 ⃗ - \vec {e_2} −e2 轴旋转的旋转矩阵。据此,我们设
R = [ c o s θ 0 − s i n θ 0 1 0 s i n θ 0 c o s θ ] d = + λ 2 R = \begin{bmatrix} cos \theta & 0 & - sin \theta \\ 0 & 1 & 0 \\ sin \theta & 0 & cos \theta \end{bmatrix}_{d = + \lambda_2} R= cosθ0sinθ010−sinθ0cosθ d=+λ2
R = [ c o s θ 0 s i n θ 0 − 1 0 s i n θ 0 − c o s θ ] d = − λ 2 R = \begin{bmatrix} cos \theta & 0 & sin \theta \\ 0 & -1 & 0 \\ sin \theta & 0 & - cos \theta \end{bmatrix}_{d = - \lambda_2} R= cosθ0sinθ0−10sinθ0−cosθ d=−λ2
代回公式 (7.3) 的第二个等式,得
d = + λ 2 { s i n θ = λ 1 − λ 3 λ 2 ⋅ n 1 ⋅ n 3 = ± λ 1 − λ 3 λ 2 ⋅ ( λ 1 2 − λ 2 2 ) ( λ 2 2 − λ 3 2 ) λ 1 2 − λ 3 2 = ± ( λ 1 2 − λ 2 2 ) ( λ 2 2 − λ 3 2 ) λ 2 ( λ 1 + λ 3 ) c o s θ = λ 1 ⋅ n 3 2 + λ 3 ⋅ n 1 2 λ 2 = λ 2 2 + λ 1 ⋅ λ 3 λ 2 ( λ 1 + λ 3 ) (7.8.1) d = + \lambda_2 \left\{\begin{aligned} 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{aligned}\right. \tag{7.8.1} d=+λ2⎩ ⎨ ⎧sinθcosθ=λ2λ1−λ3⋅n1⋅n3=±λ2λ1−λ3⋅λ12−λ32(λ12−λ22)(λ22−λ32)=±λ2(λ1+λ3)(λ12−λ22)(λ22−λ32)=λ2λ1⋅n32+λ3⋅n12=λ2(λ1+λ3)λ22+λ1⋅λ3(7.8.1)
d = − λ 2 { s i n θ = λ 1 + λ 3 λ 2 ⋅ n 1 ⋅ n 3 = ± λ 1 + λ 3 λ 2 ⋅ ( λ 1 2 − λ 2 2 ) ( λ 2 2 − λ 3 2 ) λ 1 2 − λ 3 2 = ± ( λ 1 2 − λ 2 2 ) ( λ 2 2 − λ 3 2 ) λ 2 ( λ 1 − λ 3 ) c o s θ = λ 3 ⋅ ( λ 1 2 − λ 2 2 ) − λ 1 ⋅ ( λ 2 2 − λ 3 2 ) λ 2 ⋅ ( λ 1 2 − λ 3 2 ) = λ 1 ⋅ λ 3 − λ 2 2 λ 2 ( λ 1 − λ 3 ) (7.8.2) d = - \lambda_2 \left\{\begin{aligned} 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{aligned}\right. \tag{7.8.2} d=−λ2⎩ ⎨ ⎧sinθcosθ=λ2λ1+λ3⋅n1⋅n3=±λ2λ1+λ3⋅λ12−λ32(λ12−λ22)(λ22−λ32)=±λ2(λ1−λ3)(λ12−λ22)(λ22−λ32)=λ2⋅(λ12−λ32)λ3⋅(λ12−λ22)−λ1⋅(λ22−λ32)=λ2(λ1−λ3)λ1⋅λ3−λ22(7.8.2)
由原式 d ⋅ R + t ⃗ ⋅ n ⃗ T = Λ d \cdot R + \vec t \cdot \vec n^T = \Lambda d⋅R+t⋅nT=Λ,知 d ⋅ R ⋅ n ⃗ + t ⃗ = Λ ⋅ n ⃗ d \cdot R \cdot \vec n + \vec t = \Lambda \cdot \vec n d⋅R⋅n+t=Λ⋅n,所以
t ⃗ = Λ ⋅ n ⃗ − d ⋅ R ⋅ n ⃗ = [ λ 1 0 0 0 λ 2 0 0 0 λ 3 ] [ n 1 0 n 3 ] − λ 2 [ c o s θ 0 − s i n θ 0 1 0 s i n θ 0 c o s θ ] [ n 1 0 n 3 ] = [ λ 1 ⋅ n 1 − λ 2 ⋅ n 1 ⋅ c o s θ + λ 2 ⋅ n 3 ⋅ s i n θ 0 λ 3 ⋅ n 3 − λ 2 ⋅ n 1 ⋅ s i n θ − λ 2 ⋅ n 3 ⋅ c o s θ ] = [ λ 1 ⋅ n 1 − λ 3 ⋅ n 1 3 − λ 3 ⋅ n 1 ⋅ n 3 2 0 λ 3 ⋅ n 3 − λ 1 ⋅ n 1 2 ⋅ n 3 − λ 1 ⋅ n 3 3 ] = [ λ 1 ⋅ n 1 − λ 3 ⋅ n 1 ( n 1 2 + n 3 2 ) 0 λ 3 ⋅ n 3 − λ 1 ⋅ n 3 ( n 1 2 + n 3 2 ) ] = [ λ 1 ⋅ n 1 − λ 3 ⋅ n 1 0 λ 3 ⋅ n 3 − λ 1 ⋅ n 3 ] = ( λ 1 − λ 3 ) [ n 1 0 − n 3 ] d = + λ 2 (7.9.1) \begin{aligned} \vec t &= \Lambda \cdot \vec n - d \cdot R \cdot \vec n \\ &= \begin{bmatrix} \lambda_1 & 0 & 0 \\ 0 & \lambda_2 & 0 \\ 0 & 0 & \lambda_3 \end{bmatrix} \begin{bmatrix} n_1 \\ 0 \\ n_3 \end{bmatrix} - \lambda_2 \begin{bmatrix} cos \theta & 0 & - sin \theta \\ 0 & 1 & 0 \\ sin \theta & 0 & cos \theta \end{bmatrix} \begin{bmatrix} n_1 \\ 0 \\ n_3 \end{bmatrix} \\ &= \begin{bmatrix} \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{bmatrix} \\ &= \begin{bmatrix} \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{bmatrix} \\ &= \begin{bmatrix} \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{bmatrix} = \begin{bmatrix} \lambda_1 \cdot n_1 - \lambda_3 \cdot n_1 \\ 0 \\ \lambda_3 \cdot n_3 - \lambda_1 \cdot n_3 \end{bmatrix} \\ &= (\lambda_1 - \lambda_3) \begin{bmatrix} n_1 \\ 0 \\ -n_3 \end{bmatrix}_{d = + \lambda_2} \end{aligned} \tag{7.9.1} t=Λ⋅n−d⋅R⋅n= λ1000λ2000λ3 n10n3 −λ2 cosθ0sinθ010−sinθ0cosθ n10n3 = λ1⋅n1−λ2⋅n1⋅cosθ+λ2⋅n3⋅sinθ0λ3⋅n3−λ2⋅n1⋅sinθ−λ2⋅n3⋅cosθ = λ1⋅n1−λ3⋅n13−λ3⋅n1⋅n320λ3⋅n3−λ1⋅n12⋅n3−λ1⋅n33 = λ1⋅n1−λ3⋅n1(n12+n32)0λ3⋅n3−λ1⋅n3(n12+n32) = λ1⋅n1−λ3⋅n10λ3⋅n3−λ1⋅n3 =(λ1−λ3) n10−n3 d=+λ2(7.9.1)
t ⃗ = Λ ⋅ n ⃗ − d ⋅ R ⋅ n ⃗ = [ λ 1 0 0 0 λ 2 0 0 0 λ 3 ] [ n 1 0 n 3 ] + λ 2 [ c o s θ 0 s i n θ 0 − 1 0 s i n θ 0 − c o s θ ] [ n 1 0 n 3 ] = [ λ 1 ⋅ n 1 + λ 2 ⋅ ( n 1 ⋅ c o s θ + n 3 ⋅ s i n θ ) 0 λ 3 ⋅ n 3 + λ 2 ⋅ ( n 1 ⋅ s i n θ − n 3 ⋅ c o s θ ) ] = d i a g ( n 1 , 0 , n 3 ) ⋅ [ λ 1 + λ 1 ⋅ λ 3 − λ 2 2 λ 1 − λ 3 + ( λ 1 + λ 3 ) ⋅ n 3 2 0 λ 3 + ( λ 1 + λ 3 ) ⋅ n 1 2 − λ 1 ⋅ λ 3 − λ 2 2 λ 1 − λ 3 ] = d i a g ( n 1 , 0 , n 3 ) ⋅ [ λ 1 + λ 3 0 λ 1 + λ 3 ] = ( λ 1 + λ 3 ) [ n 1 0 n 3 ] d = − λ 2 (7.9.2) \begin{aligned} \vec t &= \Lambda \cdot \vec n - d \cdot R \cdot \vec n \\ &= \begin{bmatrix} \lambda_1 & 0 & 0 \\ 0 & \lambda_2 & 0 \\ 0 & 0 & \lambda_3 \end{bmatrix} \begin{bmatrix} n_1 \\ 0 \\ n_3 \end{bmatrix} + \lambda_2 \begin{bmatrix} cos \theta & 0 & sin \theta \\ 0 & -1 & 0 \\ sin \theta & 0 & - cos \theta \end{bmatrix} \begin{bmatrix} n_1 \\ 0 \\ n_3 \end{bmatrix} \\ &= \begin{bmatrix} \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{bmatrix} \\ &= diag(n_1, 0, n_3) \cdot \begin{bmatrix} \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{bmatrix} \\ &= diag(n_1, 0, n_3) \cdot \begin{bmatrix} \lambda_1 + \lambda_3 \\ 0 \\ \lambda_1 + \lambda_3 \end{bmatrix} \\ &= (\lambda_1 + \lambda_3) \begin{bmatrix} n_1 \\ 0 \\ n_3 \end{bmatrix}_{d = - \lambda_2} \end{aligned} \tag{7.9.2} t=Λ⋅n−d⋅R⋅n= λ1000λ2000λ3 n10n3 +λ2 cosθ0sinθ0−10sinθ0−cosθ n10n3 = λ1⋅n1+λ2⋅(n1⋅cosθ+n3⋅sinθ)0λ3⋅n3+λ2⋅(n1⋅sinθ−n3⋅cosθ) =diag(n1,0,n3)⋅ λ1+λ1−λ3λ1⋅λ3−λ22+(λ1+λ3)⋅n320λ3+(λ1+λ3)⋅n12−λ1−λ3λ1⋅λ3−λ22 =diag(n1,0,n3)⋅ λ1+λ30λ1+λ3 =(λ1+λ3) n10n3 d=−λ2(7.9.2)
根据公式 (7.7)、(7.8) 、(7.9) 可知一共得到八组解: d = λ 2 d = \lambda_2 d=λ2 时, n 1 n_1 n1 和 n 3 n_3 n3 有四种符号组合,可以解得四组 ( n ⃗ , d , R , t ⃗ ) (\vec n, d, R, \vec t) (n,d,R,t); d = − λ 2 d = - \lambda_2 d=−λ2 时, n 1 n_1 n1 和 n 3 n_3 n3 有四种符号组合,可以解得四组 ( n ⃗ , d , R , t ⃗ ) (\vec n, d, R, \vec t) (n,d,R,t)。
8. 参考
- 求基础矩阵和单应矩阵前的归一化,CSDN。
- 求基础矩阵和单应矩阵前的归一化,博客园。
- 三角测量算法,CSDN。
- 分解单应矩阵,CSDN。
- 分解单应矩阵,知乎专栏。
- 分解单应矩阵,ResearchGate。
- 特征值分解与奇异值分解,知乎专栏。
- 奇异值分解,CSDN。
- 绕坐标轴旋转的旋转矩阵,CSDN。
- 绕坐标轴旋转的旋转矩阵,博客园。