三角化特征点(triangulation)方法及实现对比
文章目录
- 三角化特征点(triangulation)方法及实现对比
-
- 问题
- 解决方法
-
- 1 构造AX=0的形式，用SVD的方法进行求解
-
- 求解原理
- 代码实现
- 2 Cramer's(克莱默)法则求解Ax=b问题
-
- 求解原理
- 实现代码
- 3 单一化未知量求解
-
- 求解原理
- 实现代码
- 4 A x = b Ax=b Ax=b求解， x = ( A T A ) − 1 A T b x=\left(A^TA\right)^{-1}A^Tb x=(ATA)−1ATb
-
- 求解原理
- 代码实现
- 5 DEMO算法的几何计算深度
-
- 求解原理
- 代码实现
- 6 Tips
- 7 完整代码
- 9 其他
- 参考链接
三角化特征点(triangulation)方法及实现对比
问题
已知两帧相机在世界坐标下的位姿 [ R , t ] [R,t] [R,t], 求两帧的共视点的3维坐标。
设两帧为参考帧(reference)和当前帧(current)，共视点 p p p在两帧下的坐标分别为 x r x_r xr和 x c x_c xc，对应深度为 d r d_r dr和 d c d_c dc。图像归一化齐次坐标为 p r p_r pr和 p c p_c pc(非像素坐标)。
p r = [ u r v r 1 ] , p c = [ u c v c 1 ] p_r=\left[\begin{matrix}u_r\\v_r\\1\\\end{matrix}\right],\ \ p_c=\left[\begin{matrix}u_c\\v_c\\1\\\end{matrix}\right] pr=⎣⎡urvr1⎦⎤, pc=⎣⎡ucvc1⎦⎤
满足：
x r = R r c x c + t r c x_r=R_{rc}x_c+t_{rc} xr=Rrcxc+trc d r p r = d c R r c p c + t r c d_rp_r=d_cR_{rc}p_c+t_{rc} drpr=dcRrcpc+trc
解决方法
1 构造AX=0的形式，用SVD的方法进行求解
求解原理
设第 k k k帧相机的变换矩阵为 T k = [ R k , t k ] ∈ R 3 × 4 T_k=[R_k,t_k]∈\mathbb{R}^{3×4} Tk=[Rk,tk]∈R3×4。
则：
d k p = T k x w d_kp=T_k\bm{x}_w dkp=Tkxw
空间点的世界齐次坐标为 x w \bm{x}_w xw， d k d_k dk为观测点 p p p对应的深度值， p p p为第 k k k帧相机该像素点的归一化齐次坐标。
得： （*）
d k [ u k v k 1 ] = T k [ x w y w z w 1 ] d_k\left[\begin{matrix}u_k\\v_k\\1\\\end{matrix}\right]=T_k\left[\begin{matrix}x_w\\y_w\\z_w\\1\\\end{matrix}\right] dk⎣⎡ukvk1⎦⎤=Tk⎣⎢⎢⎡xwywzw1⎦⎥⎥⎤
由上述方程第三行可知： T 3 k T_3^k T3k表示 T k T_k Tk第三行。
d k = T 3 k x w d_k=T_3^k\bm{x}_w dk=T3kxw
将其带入(*)前两行，消去 d k d_k dk，得：
u k T 3 k x w = T 1 k x w u_kT_3^k\bm{x}_w=T_1^k\bm{x}_w ukT3kxw=T1kxw v k T 3 k x w = T 2 k x w v_kT_3^k\bm{x}_w=T_2^k\bm{x}_w vkT3kxw=T2kxw
每次观测得到以上两个方程，经过多次观测，得到以下方程，将未知量 x w \bm{x}_w xw移至一侧。
[ u 1 T 3 1 − T 1 1 v 1 T 3 1 − T 2 1 ⋮ u n T 3 n − T 1 n v n T 3 n − T 2 n ] x w = 0 → A x = 0 \left[\begin{matrix}u_1T_3^1-T_1^1\\v_1T_3^1-T_2^1\\\begin{matrix}\vdots\\u_nT_3^n-T_1^n\\v_nT_3^n-T_2^n\\\end{matrix}\\\end{matrix}\right]\bm{x}_w=0\rightarrow A\bm{x}=0 ⎣⎢⎢⎢⎢⎢⎡u1T31−T11v1T31−T21⋮unT3n−T1nvnT3n−T2n⎦⎥⎥⎥⎥⎥⎤xw=0→Ax=0
求解问题变为：
拉格朗日乘子方程:
L ( x , λ ) = x T A T A x + λ ( 1 − x T x ) L(x,\lambda)=x^TA^TAx+\lambda(1-x^Tx) L(x,λ)=xTATAx+λ(1−xTx)
我们分别对 x x x和 λ \lambda λ求偏导数：
∂ ( x , λ ) ∂ x = 2 A T A x − 2 λ x = 0 \frac{\partial(x,\lambda)}{\partial x}=2A^TAx-2\lambda x=0 ∂x∂(x,λ)=2ATAx−2λx=0 ∂ ( x , λ ) ∂ λ = 1 − x T x = 0 \frac{\partial(x,\lambda)}{\partial\lambda}=1-x^Tx=0 ∂λ∂(x,λ)=1−xTx=0
整理得：
A T A x = λ x , x T x = 1 A^TAx=\lambda x,\ \ x^Tx=1 ATAx=λx, xTx=1
由上式可知： x x x为 A T A A^TA ATA的特征向量，且模长为1。
将 A A A进行SVD分解：
A = U Σ V T , A T A = V Σ T U T U Σ V T = V Σ T Σ V T A=U\mathrm{\Sigma}V^T,\ \ A^TA=V\mathrm{\Sigma}^TU^TU\mathrm{\Sigma}V^T=VΣ^TΣV^T A=UΣVT, ATA=VΣTUTUΣVT=VΣTΣVT
可知： V V V为 A T A A^TA ATA特征向量
∣ ∣ A x ∣ ∣ 2 = x T A T A x = x T λ x = λ x T x = λ ||Ax||^2=x^TA^TAx=x^Tλx=λx^Tx=\lambda ∣∣Ax∣∣2=xTATAx=xTλx=λxTx=λ
从而可知，当 x x x值为 A T A A^TA ATA的特征向量， ∣ ∣ A x ∣ ∣ 2 ||Ax||^2 ∣∣Ax∣∣2的值为对应的特征值。
因此 ∣ ∣ A x ∣ ∣ 2 = λ m i n ||Ax||^2= λ_{min} ∣∣Ax∣∣2=λmin，问题的解 x x x值为 λ m i n \lambda_{min} λmin对应的 A T A A^TA ATA的特征向量。
在使用Eigen等库调用SVD接口时，一般会将奇异值按照从大到小的顺序排列，因此问题的解 x x x为 V V V的最后一列。解的有效性条件，对应的 λ m i n \lambda_{min} λmin非常接近于0。
1）直接对A进行SVD分解，奇异值最小对应的V的列为解值。
2）对A^TA进行SVD分解(相当于三角化)，最小特征值对应的特征向量为解值。
代码实现
auto loop_times = camera_pose.size() - start_frame_id;
cout << "********* First solution *********" << endl;
MatrixXd A((loop_times) * 2, 4);
for (int j = start_frame_id; j < loop_times; ++j) {
MatrixXd T_tmp(3, 4);
T_tmp.block<3, 3>(0, 0) = camera_pose[j].Rwc.transpose();
T_tmp.block<3, 1>(0, 3) = -camera_pose[j].Rwc.transpose() * camera_pose[j].twc;
auto P_k1 = T_tmp.block<1, 4>(0, 0);
auto P_k2 = T_tmp.block<1, 4>(1, 0);
auto P_k3 = T_tmp.block<1, 4>(2, 0);
A.block<1, 4>(2 * j, 0) = camera_pose[j].uv[0] * P_k3 - P_k1;
A.block<1, 4>(2 * j + 1, 0) = camera_pose[j].uv[1] * P_k3 - P_k2;
}
//solution1: SVD1
Matrix4d ATA = A.transpose() * A;
//对ATA进行SVD
JacobiSVD<Matrix4d> svd(ATA, ComputeFullU | ComputeFullV);
auto res_U = svd.matrixU();
auto res_V = svd.matrixV();
cout << "U=" << res_U << endl;
cout << "V=" << res_V << endl;
auto tmp = res_U.rightCols(1);
//第三项为1，归一化为标准坐标(x,y,z,1)
cout << "First result = " << endl << tmp / tmp(3) << endl;
//solution1: 对A进行SVD
JacobiSVD<MatrixXd> svd2(A, ComputeFullU | ComputeFullV);
auto res_U2 = svd2.matrixU();
auto res_V2 = svd2.matrixV();
//cout << "U=" << res_U2 << endl;
cout << "V=" << res_V2 << endl;
auto tmp2 = res_V2.rightCols(1);
//第三项为1，归一化为标准坐标(x,y,z,1)
cout << "Second result = " << endl << tmp2 / tmp2(3) << endl;
代码测试(地图观测次数对点的坐标估计的准确性的影响)
tri2.cpp加入高斯噪声
#include 编译tri2.cpp
g++ tri2.cpp `pkg-config eigen3 --libs --cflags`
运行：
./a.out
运行结果：
2 Cramer’s(克莱默)法则求解Ax=b问题
求解原理
根据公式推导如下：
d r e f ⋅ p r e f = d c u r ⋅ ( R r c ⋅ p c u r ) + t r c d_{ref}\cdot p_{ref}=d_{cur}\cdot\left(R_{rc}\cdot p_{cur}\right)+t_{rc} dref⋅pref=dcur⋅(Rrc⋅pcur)+trc
将其化为非齐次形式 A x = b Ax=b Ax=b
对公式两侧同时乘以 p r e f T p_{ref}^T prefT，得到：
d r e f ⋅ p r e f T ⋅ p r e f − d c u r ⋅ p r e f T ⋅ ( R r c ⋅ p c u r ) = p r e f T ⋅ t r c d_{ref}\cdot p_{ref}^T\cdot p_{ref}-d_{cur}\cdot p_{ref}^T\cdot\left(R_{rc}\cdot p_{cur}\right)=p_{ref}^T\cdot t_{rc} dref⋅prefT⋅pref−dcur⋅prefT⋅(Rrc⋅pcur)=prefT⋅trc
公式两侧同时乘以 ( R r c p c u r ) T \left(R_{rc}p_{cur}\right)^T (Rrcpcur)T，得到：
d r e f ⋅ ( R r c p c u r ) T ⋅ p r e f − d c u r ⋅ ( R r c p c u r ) T ⋅ ( R r c ⋅ p c u r ) = ( R r c p c u r ) T ⋅ t r c d_{ref}\cdot\left(R_{rc}p_{cur}\right)^T\cdot p_{ref}-d_{cur}\cdot\left(R_{rc}p_{cur}\right)^T\cdot\left(R_{rc}\cdot p_{cur}\right)=\left(R_{rc}p_{cur}\right)^T\cdot t_{rc} dref⋅(Rrcpcur)T⋅pref−dcur⋅(Rrcpcur)T⋅(Rrc⋅pcur)=(Rrcpcur)T⋅trc
化为非齐次形式，如下：
[ p r e f T ⋅ p r e f − p r e f T ⋅ R r c ⋅ p c u r p c u r T R r c T ⋅ p r e f − p c u r T ⋅ p c u r ] ⋅ [ d r e f d c u r ] = [ p r e f T ⋅ t r c ( R r c p c u r ) T ⋅ t r c ] \left[\begin{matrix}p_{ref}^T\cdot p_{ref}&-p_{ref}^T\cdot R_{rc}\cdot p_{cur}\\p_{cur}^TR_{rc}^T\cdot p_{ref}&-p_{cur}^T\cdot p_{cur}\\\end{matrix}\right]\cdot\left[\begin{matrix}\begin{matrix}d_{ref}\\d_{cur}\\\end{matrix}\\\end{matrix}\right]=\left[\begin{matrix}\begin{matrix}p_{ref}^T\cdot t_{rc}\\\left(R_{rc}p_{cur}\right)^T\cdot t_{rc}\\\end{matrix}\\\end{matrix}\right] [prefT⋅prefpcurTRrcT⋅pref−prefT⋅Rrc⋅pcur−pcurT⋅pcur]⋅[drefdcur]=[prefT⋅trc(Rrcpcur)T⋅trc]
对于上述两个公式组成的方程组，利用克莱默法则求解。克莱姆法则是一种求解线性方程组的方法，大多数线性代数教材都会提到。例如对于如下的线性方程组：
对于这样的方程，如果 ∣ A ∣ |A| ∣A∣不为0，方程可以通过如下方式求解。
运用克莱姆法则，这个方程组的解可以如下：
其中 D , D x , D y D,D_x,D_y D,Dx,Dy，分别是如下三个行列式：
实现代码
cout << "********* Second solution *********" << endl;
for (int j = start_frame_id + 1; j < loop_times; ++j) {
// dr * pr = dc * ( Rrc * pc ) + trc
// => [ pr^T*pr, -pr^T*R*pc ] [dr] = [pr^T*t]
// [ pc^T*R^T*pr, -pc^T*pc ] [dc] = [pc^T*R^T*t]
Matrix3d Rrc = camera_pose[j-1].Rwc.transpose() * camera_pose[j].Rwc;
Vector3d trc = camera_pose[j-1].Rwc.transpose() * (camera_pose[j].twc - camera_pose[j-1].twc);
Vector3d pr(camera_pose[j-1].uv[0], camera_pose[j-1].uv[1], 1);
Vector3d pc(camera_pose[j].uv[0], camera_pose[j].uv[1], 1);
Vector3d f2 = Rrc * pc;
double A[4];
A[0] = pr.dot( pr );
A[2] = pr.dot( f2 );
A[1] = -A[2];
A[3] = -f2.dot( f2 );
Vector2d b = Vector2d (trc.dot(pr), trc.dot(f2));
// 此处计算A的行列式
double d = A[0]*A[3] - A[1]*A[2];
Vector2d lambdavec =
Vector2d ( A[3] * b ( 0,0 ) - A[1] * b ( 1,0 ),
-A[2] * b ( 0,0 ) + A[0] * b ( 1,0 )) /d;
/*Vector3d */pr = lambdavec ( 0,0 ) * pr;
/*Vector3d */pc = trc + lambdavec ( 1,0 ) * f2;
pr = camera_pose[j-1].Rwc * pr + camera_pose[j-1].twc;
pc = camera_pose[j-1].Rwc * pc + camera_pose[j-1].twc;
cout << "ir: " << j << " result1: " << pr << endl;
cout << "ir: " << j << " result2: " << pc << endl;
Vector3d d_esti = ( pr+pc ) / 2.0; // 三角化算得的深度向量
//double depth_estimation = d_esti.norm(); // 深度值
cout << "ir: " << j << " final result: " << d_esti << endl;
}
3 单一化未知量求解
求解原理
根据公式
d r p r = d c R r c p c + t r c d_rp_r=d_cR_{rc}p_c+t_{rc} drpr=dcRrcpc+trc
左右两侧都乘以 p r × p_r^\times pr×, p r × p_r^\times pr×为p_r的反对称矩阵，得：
d r p r × p r = d c p r × R r c p c + p r × t r c = 0 d_r{p_r^\times p}_r=d_c{p_r^\times R}_{rc}p_c+p_r^\times t_{rc}=0 drpr×pr=dcpr×Rrcpc+pr×trc=0
从而方程中只剩 d c d_c dc一个未知量，求解 d c d_c dc，从而得该点的三维坐标。
实现代码
//solution3: drpr = dc*R*pc+t => dc*pr^*R*pc + pr^ * t = 0
//dr*pr = dc*Rrc*pc + trc
cout << "********* Third solution *********" << endl;
for (int j = start_frame_id + 1; j < loop_times; ++j) {
Matrix3d Rrc = camera_pose[j-1].Rwc.transpose() * camera_pose[j].Rwc;
Vector3d trc = camera_pose[j-1].Rwc.transpose() * (camera_pose[j].twc - camera_pose[j-1].twc);
Vector3d pr(camera_pose[j-1].uv[0], camera_pose[j-1].uv[1], 1);
Vector3d pc(camera_pose[j].uv[0], camera_pose[j].uv[1], 1);
Vector3d f1 = pr.cross(Rrc*pc);
Vector3d f2 = -pr.cross(trc);
Vector3d dc(f2[0]/f1[0], f2[1]/f1[1], f2[2]/f1[2]);
cout << "dc: " << dc << endl;
double dc_ave = (dc[0] + dc[1] + dc[2])/3;
Vector3d xc = dc_ave * pc;
xc = camera_pose[j].Rwc * xc + camera_pose[j].twc;
cout << "ir: " << j << " result: " << xc << endl;
}
4 A x = b Ax=b Ax=b求解， x = ( A T A ) − 1 A T b x=\left(A^TA\right)^{-1}A^Tb x=(ATA)−1ATb
求解原理
根据公式
d r p r = d c R r c p c + t r c d_rp_r=d_cR_{rc}p_c+t_{rc} drpr=dcRrcpc+trc
写成矩阵形式：
[ p r − R r c p c ] [ d r d c ] = t r c → A d = b \left[\begin{matrix}p_r&-R_{rc}p_c\\\end{matrix}\right]\left[\begin{matrix}d_r\\d_c\\\end{matrix}\right]=t_{rc}\rightarrow Ad=b [pr−Rrcpc][drdc]=trc→Ad=b
解：
d = ( A T A ) − 1 A T b d=\left(A^TA\right)^{-1}A^Tb d=(ATA)−1ATb
代码实现
cout << "********* Forth solution *********" << endl;
for (int j = start_frame_id + 1; j < loop_times; ++j) {
Matrix3d Rrc = camera_pose[j-1].Rwc.transpose() * camera_pose[j].Rwc;
Vector3d trc = camera_pose[j-1].Rwc.transpose() * (camera_pose[j].twc - camera_pose[j-1].twc);
Vector3d pr(camera_pose[j-1].uv[0], camera_pose[j-1].uv[1], 1);
Vector3d pc(camera_pose[j].uv[0], camera_pose[j].uv[1], 1);
MatrixXd A(3, 2);
A.block<3, 1>(0, 0) = pr;
A.block<3, 1>(0, 1) = -Rrc * pc;
Vector2d d = (A.transpose() * A).inverse() * A.transpose() * trc;
/* Vector3d */pr = d[0] * pr;
/* Vector3d */pc = d[1] * pc;
pr = camera_pose[j-1].Rwc * pr + camera_pose[j-1].twc;
pc = camera_pose[j].Rwc * pc + camera_pose[j].twc;
cout << "ir: " << j << " result1: " << pr << endl;
cout << "ir: " << j << " result2: " << pc << endl;
}
5 DEMO算法的几何计算深度
求解原理
利用平行关系：
[u0,v0], [u1,v1]均为归一化平面坐标
作者将last相机位置沿O_Last->X方向移动tz到O_Last’，从而将O_Last移动到与O_Cur z方向平行的方向，此时， t x ′ = t x − t z ∗ u 0 ， t y ′ = t y − t z ∗ v 0 tx'=tx-tz*u0，ty'=ty-tz*v0 tx′=tx−tz∗u0，ty′=ty−tz∗v0
在O_Last‘作与O_Cur->X平行的辅助线，此时，[u0,v0], [u1,v1], X, O_Last, O_Cur, O_Last’都在同一平面上。
[u1,v1]->[u0,v0]平行于O_Last‘->O_Cur。
A B C O L ′ = O L ′ O C X D → A B C O L ′ c o s θ = O L ′ O C X D c o s θ \frac{AB}{CO_L^\prime}=\frac{O_L^\prime O_C}{XD}\rightarrow\frac{AB}{CO_L^\prime c o s\theta}=\frac{O_L^\prime O_C}{XDcos\theta} COL′AB=XDOL′OC→COL′cosθAB=XDcosθOL′OC
( u 1 − u 0 ) 2 + ( v 1 − v 0 ) 2 1 = ( t z u 0 − t x ) 2 + ( t z v 0 − t y ) 2 Z \frac{\sqrt{\left(u_1-u_0\right)^2+\left(v_1-v_0\right)^2}}{1}=\frac{\sqrt{\left({t_zu}_0-t_x\right)^2+\left({t_zv}_0-t_y\right)^2}}{Z} 1(u1−u0)2+(v1−v0)2=Z(tzu0−tx)2+(tzv0−ty)2
Z = ( t z u 0 − t x ) 2 + ( t z v 0 − t y ) 2 ( u 1 − u 0 ) 2 + ( v 1 − v 0 ) 2 Z=\frac{\sqrt{\left({t_zu}_0-t_x\right)^2+\left({t_zv}_0-t_y\right)^2}}{\sqrt{\left(u_1-u_0\right)^2+\left(v_1-v_0\right)^2}} Z=(u1−u0)2+(v1−v0)2(tzu0−tx)2+(tzv0−ty)2
代码中为什么除以cos(atan2(tz * v1 - ty, tz * u1 - tx) - atan2(v0 - v1, u0 - u1))未知。
代码实现
cout << "********* Fifth solution *********" << endl;
int ind_rec = start_frame_id;
for (int j = start_frame_id; j < loop_times; ++j) {
Vector3d dt = camera_pose[j].twc - camera_pose[ind_rec].twc;
double dis = sqrt(pow(dt[0], 2) + pow(dt[1], 2) + pow(dt[2], 2));
if (dis > 0.3) {
Matrix3d Rrc = camera_pose[ind_rec].Rwc.transpose() * camera_pose[j].Rwc;
Vector3d trc = camera_pose[ind_rec].Rwc.transpose()
* (camera_pose[j].twc - camera_pose[ind_rec].twc);
Vector3d pr(camera_pose[ind_rec].uv[0], camera_pose[ind_rec].uv[1], 1);
Vector3d pc(camera_pose[j].uv[0], camera_pose[j].uv[1], 1);
//将pc的坐标系旋转到与rec坐标系方向一致
//pcr << pcr[0]/pcr[2], pcr[1]/pcr[2], 1;
Vector3d pcr = Rrc * pc;
double u1 = camera_pose[ind_rec].uv[0];
double v1 = camera_pose[ind_rec].uv[1];
double u0 = pcr[0]/pcr[2];
double v0 = pcr[1]/pcr[2];
double tx = trc[0];
double ty = trc[1];
double tz = trc[2];
double delta = sqrt((v0 - v1) * (v0 - v1) + (u0 - u1) * (u0 - u1))
* cos(atan2(tz * v1 - ty, tz * u1 - tx) - atan2(v0 - v1, u0 - u1));
//double delta = sqrt((v0 - v1) * (v0 - v1) + (u0 - u1) * (u0 - u1));
double dr = sqrt((tz * u0 - tx) * (tz * u0 - tx) + (tz * v0 - ty) * (tz * v0 - ty)) / delta;
pr = dr * pr;
pr = camera_pose[ind_rec].Rwc * pr + camera_pose[ind_rec].twc;
cout << "ir: " << j << " result: " << pr << endl;
ind_rec = j;
}
}
6 Tips
可三角化求解条件，两帧之间有足够的漂移运动。
点云深度的恢复，需要两帧相机之间有足够的运动漂移，小视差(近似纯旋转)的场景下，尺度不确定，深度解值无限。
设纯旋转： T = [ R 0 ] T=[\begin{matrix}R&0\\\end{matrix}] T=[R0]
图像归一化齐次坐标为 p r p_r pr和 p c p_c pc(非像素坐标)。
p r = [ u r v r 1 ] , p c = [ u c v c 1 ] p_r=\left[\begin{matrix}u_r\\v_r\\1\\\end{matrix}\right],\ \ p_c=\left[\begin{matrix}u_c\\v_c\\1\\\end{matrix}\right] pr=⎣⎡urvr1⎦⎤, pc=⎣⎡ucvc1⎦⎤
满足：
x r = R r c x c x_r=R_{rc}x_c xr=Rrcxc d r p r = d c R r c p c d_rp_r=d_cR_{rc}p_c drpr=dcRrcpc
d r d_r dr为目标求解对象，从而得到地图点的3D坐标。
从上述公式可知，即使是有多帧观测，但是均是小漂移场景时，设其组成下述方程：
d r p r = d c R r c p c \bm{d}_r\bm{p}_r=\bm{d}_c\bm{R}_{rc}\bm{p}_c drpr=dcRrcpc d r p r = [ d r 1 p r 1 , d r 2 p r 2 , d r 3 p r 3 , ⋯ ] T \bm{d}_r\bm{p}_r=[d_{r1}p_{r1}, d_{r2}p_{r2}, d_{r3}p_{r3},⋯]^T drpr=[dr1pr1,dr2pr2,dr3pr3,⋯]T右侧同理。
此时左右同时乘以任意常数 c c c，仍满足方程，因此解值无限， c d r c\bm{d}_r cdr。
即使采用第一种方法来解算：
[ u 1 T 3 1 − T 1 1 v 1 T 3 1 − T 2 1 ⋮ u n T 3 n − T 1 n v n T 3 n − T 2 n ] x w = [ u 1 T 3 , 1 1 − T 1 , 1 1 u 1 T 3 , 2 1 − T 1 , 2 1 v 1 T 3 , 1 1 − T 2 , 1 1 v 1 T 3 , 2 1 − T 2 , 2 1 u 1 T 3 , 3 1 − T 1 , 3 1 u 1 T 3 , 4 1 − T 1 , 4 1 v 1 T 3 , 3 1 − T 2 , 3 1 v 1 T 3 , 4 1 − T 2 , 4 1 ⋮ ⋮ u n T 3 , 1 n − T 1 , 1 n u n T 3 , 2 n − T 1 , 2 n ⋮ ⋮ u n T 3 , 3 n − T 1 , 3 n u n T 3 , 4 n − T 1 , 4 n ] [ x y z f ] = 0 \left[\begin{matrix}u_1T_3^1-T_1^1\\v_1T_3^1-T_2^1\\\begin{matrix}\vdots\\u_nT_3^n-T_1^n\\v_nT_3^n-T_2^n\\\end{matrix}\\\end{matrix}\right]\bm{x}_w=\left[\begin{matrix}\begin{matrix}u_1T_{3,1}^1-T_{1,1}^1&u_1T_{3,2}^1-T_{1,2}^1\\v_1T_{3,1}^1-T_{2,1}^1&v_1T_{3,2}^1-T_{2,2}^1\\\end{matrix}&\begin{matrix}u_1T_{3,3}^1-T_{1,3}^1&u_1T_{3,4}^1-T_{1,4}^1\\v_1T_{3,3}^1-T_{2,3}^1&v_1T_{3,4}^1-T_{2,4}^1\\\end{matrix}\\\begin{matrix}\vdots&\vdots\\u_nT_{3,1}^n-T_{1,1}^n&u_nT_{3,2}^n-T_{1,2}^n\\\end{matrix}&\begin{matrix}\vdots&\vdots\\u_nT_{3,3}^n-T_{1,3}^n&u_nT_{3,4}^n-T_{1,4}^n\\\end{matrix}\\\end{matrix}\right]\left[\begin{matrix}x\\y\\\begin{matrix}z\\f\\\end{matrix}\\\end{matrix}\right]=0 ⎣⎢⎢⎢⎢⎢⎡u1T31−T11v1T31−T21⋮unT3n−T1nvnT3n−T2n⎦⎥⎥⎥⎥⎥⎤xw=⎣⎢⎢⎢⎡u1T3,11−T1,11v1T3,11−T2,11u1T3,21−T1,21v1T3,21−T2,21⋮unT3,1n−T1,1n⋮unT3,2n−T1,2nu1T3,31−T1,31v1T3,31−T2,31u1T3,41−T1,41v1T3,41−T2,41⋮unT3,3n−T1,3n⋮unT3,4n−T1,4n⎦⎥⎥⎥⎤⎣⎢⎢⎡xyzf⎦⎥⎥⎤=0
当 T = [ R 0 ] T=[\begin{matrix}R&0\\\end{matrix}] T=[R0]时，上述方程为： A A A阵的最后一列为0。
[ u 1 T 3 , 1 1 − T 1 , 1 1 u 1 T 3 , 2 1 − T 1 , 2 1 v 1 T 3 , 1 1 − T 2 , 1 1 v 1 T 3 , 2 1 − T 2 , 2 1 u 1 T 3 , 3 1 − T 1 , 3 1 0 v 1 T 3 , 3 1 − T 2 , 3 1 0 ⋮ ⋮ u n T 3 , 1 n − T 1 , 1 n u n T 3 , 2 n − T 1 , 2 n ⋮ ⋮ u n T 3 , 3 n − T 1 , 3 n 0 ] [ x y z f ] = 0 \left[\begin{matrix}\begin{matrix}u_1T_{3,1}^1-T_{1,1}^1&u_1T_{3,2}^1-T_{1,2}^1\\v_1T_{3,1}^1-T_{2,1}^1&v_1T_{3,2}^1-T_{2,2}^1\\\end{matrix}&\begin{matrix}u_1T_{3,3}^1-T_{1,3}^1&0\\v_1T_{3,3}^1-T_{2,3}^1&0\\\end{matrix}\\\begin{matrix}\vdots&\vdots\\u_nT_{3,1}^n-T_{1,1}^n&u_nT_{3,2}^n-T_{1,2}^n\\\end{matrix}&\begin{matrix}\vdots&\vdots\\u_nT_{3,3}^n-T_{1,3}^n&0\\\end{matrix}\\\end{matrix}\right]\left[\begin{matrix}x\\y\\\begin{matrix}z\\f\\\end{matrix}\\\end{matrix}\right]=0 ⎣⎢⎢⎢⎡u1T3,11−T1,11v1T3,11−T2,11u1T3,21−T1,21v1T3,21−T2,21⋮unT3,1n−T1,1n⋮unT3,2n−T1,2nu1T3,31−T1,31v1T3,31−T2,3100⋮unT3,3n−T1,3n⋮0⎦⎥⎥⎥⎤⎣⎢⎢⎡xyzf⎦⎥⎥⎤=0 s . t . ∣ ∣ x ∣ ∣ 2 = 1 s.t.\ \ ||x||^2=1 s.t. ∣∣x∣∣2=1
此时上述方程的解无限，f为值不影响结果。从而可知尺度不确定。
7 完整代码
1 #include <iostream>
2 #include <vector>
3 #include <random>
4 #include <Eigen/Core>
5 #include <Eigen/Geometry>
6 #include <Eigen/Eigenvalues>
7
8 using namespace Eigen;
9 using namespace std;
10
11 struct Pose {
12 Pose(Matrix3d R, Vector3d t) : Rwc(R), qwc(R), twc(t) {};
13
14 Matrix3d Rwc;
15 Quaterniond qwc;
16 Vector3d twc;
17
18 Vector2d uv;
19 };
20
21 int main() {
22 int poseNums = 10;
23 double radius = 8;
24 double fx = 1.;
25 double fy = 1.;
26
27 vector<Pose> camera_pose;
28 for (int n = 0; n < poseNums; ++n) {
29 double theta = n * 2 * M_PI / (poseNums * 4); // 1/4 ??
30 Matrix3d R;
31 R = AngleAxisd(theta, Vector3d::UnitZ());
32 Vector3d t = Vector3d(radius * cos(theta) - radius,
33 radius * sin(theta), 1 * sin(2 * theta));
34 camera_pose.push_back(Pose(R, t));
35 }
36
37 default_random_engine generator;
38 uniform_real_distribution<double> xy_rand(-4, 4.0);
39 uniform_real_distribution<double> z_rand(8., 10.);
40 double tx = xy_rand(generator);
41 double ty = xy_rand(generator);
42 double tz = z_rand(generator);
43
44 Vector3d Pw(tx, ty, tz);
45 cout << "Ground truth:" << Pw.transpose() << endl;
46
47 int start_frame_id = 3;
48 int end_frame_id = poseNums;
49 //default_random_engine generator;
50 for (int i = start_frame_id; i < end_frame_id; ++i) {
51 Matrix3d Rcw = camera_pose[i].Rwc.transpose();
52 Vector3d Pc = Rcw * (Pw - camera_pose[i].twc);
53 //normal_distribution randx(0, 0.01);
56 //normal_distribution randy(0, 0.01);
57 //normal_distribution randz(0, 0.01);
58
54 double x = Pc.x();
55 double y = Pc.y();
56 double z = Pc.z();
57
58 camera_pose[i].uv = Vector2d(x / z, y / z);
59 }
60
61 //solution1: SVD
62 Vector3d P_est;
63 P_est.setZero();
64 /* your code begin */
65 auto loop_times = camera_pose.size() - start_frame_id;
66 MatrixXd A((loop_times) * 2, 4);
67 for (int j = start_frame_id; j < loop_times; ++j) {
68 MatrixXd T_tmp(3, 4);
69 T_tmp.block<3, 3>(0, 0) = camera_pose[j].Rwc.transpose();
70 T_tmp.block<3, 1>(0, 3) = -camera_pose[j].Rwc.transpose() * camera_pose[j].twc;
71
72 auto P_k1 = T_tmp.block<1, 4>(0, 0);
73 auto P_k2 = T_tmp.block<1, 4>(1, 0);
74 auto P_k3 = T_tmp.block<1, 4>(2, 0);
75
76 A.block<1, 4>(2 * j, 0) = camera_pose[j].uv[0] * P_k3 - P_k1;
77 A.block<1, 4>(2 * j + 1, 0) = camera_pose[j].uv[1] * P_k3 - P_k2;
78 }
79
80 //solution1: SVD1
81 cout << "********* First solution *********" << endl;
82 //对ATA进行SVD
83 Matrix4d ATA = A.transpose() * A;
84 JacobiSVD<Matrix4d> svd(ATA, ComputeFullU | ComputeFullV);
85 auto res_U = svd.matrixU();
86 auto res_V = svd.matrixV();
87 cout << "U=" << res_U << endl;
88 cout << "V=" << res_V << endl;
89 auto tmp = res_U.rightCols(1);
90 //第三项为1，归一化为标准坐标(x,y,z,1)
91 cout << "First result = " << endl << tmp / tmp(3) << endl;
92
93 //solution1: SVD2
94 //对A进行SVD
95 JacobiSVD<MatrixXd> svd2(A, ComputeFullU | ComputeFullV);
96 auto res_U2 = svd2.matrixU();
97 auto res_V2 = svd2.matrixV();
98 //cout << "U=" << res_U2 << endl;
99 cout << "V=" << res_V2 << endl;
100 auto tmp2 = res_V2.rightCols(1);
101 //第三项为1，归一化为标准坐标(x,y,z,1)
102 cout << "Second result = " << endl << tmp2 / tmp2(3) << endl;
103
104 //solution2: Cramer's
105 cout << "********* Second solution *********" << endl;
106 for (int j = start_frame_id + 1; j < loop_times; ++j) {
107 // dr * pr = dc * ( Rrc * pc ) + trc
108 // => [ pr^T*pr, -pr^T*R*pc ] [dr] = [pr^T*t]
109 // [ pc^T*R^T*pr, -pc^T*pc ] [dc] = [pc^T*R^T*t]
110 Matrix3d Rrc = camera_pose[j-1].Rwc.transpose() * camera_pose[j].Rwc;
111 Vector3d trc = camera_pose[j-1].Rwc.transpose() * (camera_pose[j].twc - camera_pose[j-1].twc) ;
112 Vector3d pr(camera_pose[j-1].uv[0], camera_pose[j-1].uv[1], 1);
113 Vector3d pc(camera_pose[j].uv[0], camera_pose[j].uv[1], 1);
114
115 Vector3d f2 = Rrc * pc;
116 double A[4];
117 A[0] = pr.dot( pr );
118 A[2] = pr.dot( f2 );
119 A[1] = -A[2];
120 A[3] = -f2.dot( f2 );
121
122 Vector2d b = Vector2d (trc.dot(pr), trc.dot(f2));
123
124 // 此处计算A的行列式
125 double d = A[0]*A[3] - A[1]*A[2];
126 Vector2d lambdavec =
127 Vector2d ( A[3] * b ( 0,0 ) - A[1] * b ( 1,0 ),
128 -A[2] * b ( 0,0 ) + A[0] * b ( 1,0 )) /d;
129 /*Vector3d */pr = lambdavec ( 0,0 ) * pr;
130 /*Vector3d */pc = trc + lambdavec ( 1,0 ) * f2;
131
132 pr = camera_pose[j-1].Rwc * pr + camera_pose[j-1].twc;
133 pc = camera_pose[j-1].Rwc * pc + camera_pose[j-1].twc;
134 cout << "ir: " << j << " result1: " << pr << endl;
135 cout << "ir: " << j << " result2: " << pc << endl;
136
137 Vector3d d_esti = ( pr+pc ) / 2.0; // 三角化算得的深度向量
138 //double depth_estimation = d_esti.norm(); // 深度值
139
140 cout << "ir: " << j << " final result: " << d_esti << endl;
141 }
142
143 //solution3: drpr = dc*R*pc+t => dc*pr^*R*pc + pr^ * t = 0
144 //dr*pr = dc*Rrc*pc + trc
145 cout << "********* Third solution *********" << endl;
146 for (int j = start_frame_id + 1; j < loop_times; ++j) {
147 Matrix3d Rrc = camera_pose[j-1].Rwc.transpose() * camera_pose[j].Rwc;
148 Vector3d trc = camera_pose[j-1].Rwc.transpose() * (camera_pose[j].twc - camera_pose[j-1].twc) ;
149 Vector3d pr(camera_pose[j-1].uv[0], camera_pose[j-1].uv[1], 1);
150 Vector3d pc(camera_pose[j].uv[0], camera_pose[j].uv[1], 1);
151
152 Vector3d f1 = pr.cross(Rrc*pc);
153 Vector3d f2 = -pr.cross(trc);
154
155 Vector3d dc(f2[0]/f1[0], f2[1]/f1[1], f2[2]/f1[2]);
156 cout << "dc: " << dc << endl;
157
158 double dc_ave = (dc[0] + dc[1] + dc[2])/3;
159 Vector3d xc = dc_ave * pc;
160
161 xc = camera_pose[j].Rwc * xc + camera_pose[j].twc;
162 cout << "ir: " << j << " result: " << xc << endl;
163 }
164
165 //solution4: (ATA)^-1*ATb
166 //dr*pr = dc*Rrc*pc + trc
167 // A = [pr -Rrc*pc] b = [trc], d = [dr, dc]
168 // Ad = b
169 cout << "********* Forth solution *********" << endl;
170 for (int j = start_frame_id + 1; j < loop_times; ++j) {
171 Matrix3d Rrc = camera_pose[j-1].Rwc.transpose() * camera_pose[j].Rwc;
172 Vector3d trc = camera_pose[j-1].Rwc.transpose() * (camera_pose[j].twc - camera_pose[j-1].twc) ;
173 Vector3d pr(camera_pose[j-1].uv[0], camera_pose[j-1].uv[1], 1);
174 Vector3d pc(camera_pose[j].uv[0], camera_pose[j].uv[1], 1);
175
176 MatrixXd A(3, 2);
177 A.block<3, 1>(0, 0) = pr;
178 A.block<3, 1>(0, 1) = -Rrc * pc;
179
180 Vector2d d = (A.transpose() * A).inverse() * A.transpose() * trc;
181
182 /* Vector3d */pr = d[0] * pr;
183 /* Vector3d */pc = d[1] * pc;
184
185 pr = camera_pose[j-1].Rwc * pr + camera_pose[j-1].twc;
186 pc = camera_pose[j].Rwc * pc + camera_pose[j].twc;
187 cout << "ir: " << j << " result1: " << pr << endl;
188 cout << "ir: " << j << " result2: " << pc << endl;
189 }
190
191 //solution5: demo 三角化几何计算深度
192 //dr*pr = dc*Rrc*pc + trc
193 //r:1 c:0
194 cout << "********* Fifth solution *********" << endl;
195 int ind_rec = start_frame_id;
196 for (int j = start_frame_id; j < loop_times; ++j) {
197 Vector3d dt = camera_pose[j].twc - camera_pose[ind_rec].twc;
198 double dis = sqrt(pow(dt[0], 2) + pow(dt[1], 2) + pow(dt[2], 2));
199 if (dis > 0.3) {
200 Matrix3d Rrc = camera_pose[ind_rec].Rwc.transpose() * camera_pose[j].Rwc;
201 Vector3d trc = camera_pose[ind_rec].Rwc.transpose()
202 * (camera_pose[j].twc - camera_pose[ind_rec].twc);
203 Vector3d pr(camera_pose[ind_rec].uv[0], camera_pose[ind_rec].uv[1], 1);
204 Vector3d pc(camera_pose[j].uv[0], camera_pose[j].uv[1], 1);
205 //将pc的坐标系旋转到与rec坐标系方向一致
206 //pcr << pcr[0]/pcr[2], pcr[1]/pcr[2], 1;
207 Vector3d pcr = Rrc * pc;
208
209 double u1 = camera_pose[ind_rec].uv[0];
210 double v1 = camera_pose[ind_rec].uv[1];
211 double u0 = pcr[0]/pcr[2];
212 double v0 = pcr[1]/pcr[2];
213
214 double tx = trc[0];
215 double ty = trc[1];
216 double tz = trc[2];
217
218 //double delta = sqrt((v0 - v1) * (v0 - v1) + (u0 - u1) * (u0 - u1))
219 // * cos(atan2(tz * v1 - ty, tz * u1 - tx) - atan2(v0 - v1, u0 - u1));
220 double delta = sqrt((v0 - v1) * (v0 - v1) + (u0 - u1) * (u0 - u1));
221 double dr = sqrt((tz * u0 - tx) * (tz * u0 - tx) + (tz * v0 - ty) * (tz * v0 - ty)) / delta ;
222 pr = dr * pr;
223
224 pr = camera_pose[ind_rec].Rwc * pr + camera_pose[ind_rec].twc;
225 cout << "ir: " << j << " result: " << pr << endl;
226
227 ind_rec = j;
228 }
229 }
230
231 return 0;
232 }
编译：
g++ triangulation.cpp `pkg-config eigen3 --libs --cflags` -o test
运行：
./test
结果：
9 其他
已知多次地图点的三维观测值，求其最佳坐标
地图点坐标优化：
一个地图点，被多个相机观测到或者在不同位姿下有多个3D观测值(深度相机/雷达)，通过该计算该三维位姿的最佳三维估计。
问题：
m i n x W ∑ i = 0 N ∣ ∣ x W − T i x C ∣ ∣ 2 \underset{x_W} {min}{\sum_{i=0}^{N}||{x_W-T_ix_C}}||^2 xWmini=0∑N∣∣xW−TixC∣∣2
通过求导可知，上述问题的极小值就为多次观测的平均值：
x W = ∑ i = 0 N T i x C x_W=\sum_{i=0}^{N}{T_ix_C} xW=i=0∑NTixC
tri2_ave.cpp
#include 参考链接
三角测距方法：https://blog.csdn.net/KYJL888/article/details/107222533/
克莱默法则-https://blog.csdn.net/zbq_tt5/article/details/90043320
参考SVO-https://blog.csdn.net/luoshi006/article/details/80792043