文章目录
三角化特征点(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 <iostream>
#include <random>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
using namespace Eigen;
using namespace std;
double rx=0; double ry=0; double rz=0;
double tx=0; double ty=0; double tz=0;
default_random_engine poseRand(time(NULL));
//poseRand.seed(time(NULL));
normal_distribution<double> rxr(0.2, 1);
normal_distribution<double> ryr(0.1, 1);
normal_distribution<double> rzr(0.2, 1);
normal_distribution<double> txr(0.1, 1);
normal_distribution<double> tyr(0.2, 1);
normal_distribution<double> tzr(0.1, 1);
int main()
{
default_random_engine pointRand;
uniform_real_distribution<double> xy_rand(-4, 4.0);
uniform_real_distribution<double> z_rand(8., 10.);
double x = xy_rand(pointRand);
double y = xy_rand(pointRand);
double z = z_rand(pointRand);
Vector3d pointW(x, y, z);
cout << "Ground truth:" << pointW.transpose() << endl;
int irNum = 7;
int poseNum[irNum] = {2, 10, 20, 100, 200, 500, 1000};
for (int ir=0; ir < irNum; ir++) {
Eigen::MatrixXd mapPointA(2*poseNum[ir],4);
Eigen::MatrixXd t(1, 4);
t << -1*100, 0, pointW[0]/pointW[2]*100, 0;
mapPointA.block<1, 4>(0, 0) = t;
t << 0, -1*100, pointW[1]/pointW[2]*100, 0;
mapPointA.block<1, 4>(1, 0) = t;
for (int i = 1; i < poseNum[ir]; i++) {
rx += rxr(poseRand);
ry += ryr(poseRand);
rz += rzr(poseRand);
tx += txr(poseRand);
ty += tyr(poseRand);
tz += tzr(poseRand);
double srx = sin(rx);
double crx = cos(rx);
double sry = sin(ry);
double cry = cos(ry);
double srz = sin(rz);
double crz = cos(rz);
normal_distribution<double> pr(0, 0.2);
Vector3d point;
//point[2] = crx*sry*pointW[0] - srx*pointW[1] + crx*cry*pointW[2] - tz;
//point[0] = (crz*cry + srz*srx*sry)*pointW[0] + srz*crx*pointW[1] + (-crz*sry + srz*srx*cry)*pointW[2] - tx;
//point[1] = (-srz*cry + crz*srx*sry)*pointW[0] + crz*crx*pointW[1] + (srz*sry + crz*srx*cry)*pointW[2] - ty;
point[2] = crx*sry*pointW[0] - srx*pointW[1] + crx*cry*pointW[2] - tz + pr(poseRand);
point[0] = (crz*cry + srz*srx*sry)*pointW[0] + srz*crx*pointW[1] + (-crz*sry + srz*srx*cry)*pointW[2] - tx + pr(poseRand);
point[1] = (-srz*cry + crz*srx*sry)*pointW[0] + crz*crx*pointW[1] + (srz*sry + crz*srx*cry)*pointW[2] - ty + pr(poseRand);
t << point[0]/point[2]*crx*sry - (crz*cry + srz*srx*sry),
-point[0]/point[2]*srx - srz*crx,
point[0]/point[2]*crx*cry - (-crz*sry + srz*srx*cry),
-point[0]/point[2]*tz + tx;
mapPointA.block<1, 4>(2*i, 0) = t;
t << point[1]/point[2]*crx*sry - (-srz*cry + crz*srx*sry),
-point[1]/point[2]*srx - crz*crx,
point[1]/point[2]*crx*cry - (srz*sry + crz*srx*cry),
-point[1]/point[2]*tz + ty;
mapPointA.block<1, 4>(2*i+1, 0) = t;
}
Eigen::Matrix4d ATA = mapPointA.transpose()*mapPointA;
Eigen::JacobiSVD<Eigen::MatrixXd> svd(ATA,
Eigen::ComputeFullU|Eigen::ComputeFullV);
auto res_U = svd.matrixU();
auto lambda = svd.singularValues();
Vector3d pointE(x, y, z);
//if (lambda[-1] < 0.5 && lambda[-1] != 0) {
auto tmp = res_U.rightCols(1);
pointE[0] = double(tmp(0) / tmp(3));
pointE[1] = double(tmp(1) / tmp(3));
pointE[2] = double(tmp(2) / tmp(3));
//}
cout << "point estimate: " << pointE.transpose() << endl;
cout << "pose Num is: " << poseNum[ir] << endl;
cout << "estimate error rmse: " << sqrt(pow(pointE[0]-pointW[0],2) + pow(pointE[1]-pointW[1],2) + pow(pointE[2]-pointW[2],2)) << endl;
cout << "************iret: " << ir << " end*****************" << endl;
}
}
编译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<double> randx(0, 0.01);
56 //normal_distribution<double> randy(0, 0.01);
57 //normal_distribution<double> 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 <iostream>
#include <random>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
using namespace Eigen;
using namespace std;
double rx=0; double ry=0; double rz=0;
double tx=0; double ty=0; double tz=0;
default_random_engine poseRand(time(NULL));
//poseRand.seed(time(NULL));
normal_distribution<double> rxr(0.2, 1);
normal_distribution<double> ryr(0.1, 1);
normal_distribution<double> rzr(0.2, 1);
normal_distribution<double> txr(0.1, 1);
normal_distribution<double> tyr(0.2, 1);
normal_distribution<double> tzr(0.1, 1);
int main()
{
default_random_engine pointRand;
uniform_real_distribution<double> xy_rand(-4, 4.0);
uniform_real_distribution<double> z_rand(8., 10.);
double x = xy_rand(pointRand);
double y = xy_rand(pointRand);
double z = z_rand(pointRand);
Vector3d pointW(x, y, z);
Vector3d pointE(x, y, z);
cout << "Ground truth:" << pointW.transpose() << endl;
int irNum = 7;
int poseNum[irNum] = {2, 10, 20, 100, 200, 500, 1000};
double xnAve = 0; double ynAve = 0; double znAve = 0;
for (int ir=0; ir < irNum; ir++) {
// pw = RT * pc + t
// pc = R * (pw - t)
for (int i = 1; i < poseNum[ir]; i++) {
rx += rxr(poseRand);
ry += ryr(poseRand);
rz += rzr(poseRand);
tx += txr(poseRand);
ty += tyr(poseRand);
tz += tzr(poseRand);
double srx = sin(rx);
double crx = cos(rx);
double sry = sin(ry);
double cry = cos(ry);
double srz = sin(rz);
double crz = cos(rz);
normal_distribution<double> pr(0, 0.2);
Vector3d point;
//point[2] = crx*sry*(pointW[0]-tx) - srx*(pointW[1]-ty) + crx*cry*(pointW[2]-tz);
//point[0] = (crz*cry + srz*srx*sry)*pointW[0] + srz*crx*pointW[1] + (-crz*sry + srz*srx*cr y)*pointW[2];
//point[1] = (-srz*cry + crz*srx*sry)*pointW[0] + crz*crx*pointW[1] + (srz*sry + crz*srx*cr y)*pointW[2];
double xn = pr(poseRand);
double yn = pr(poseRand);
double zn = pr(poseRand);
point[2] = crx*sry*(pointW[0]-tx) - srx*(pointW[1]-ty) + crx*cry*(pointW[2]-tz) + xn;
point[0] = (crz*cry + srz*srx*sry)*(pointW[0]-tx) + srz*crx*(pointW[1]-ty) + (-crz*sry + srz*srx*cry)*(pointW[2]-tz) + yn;
point[1] = (-srz*cry + crz*srx*sry)*(pointW[0]-tx) + crz*crx*(pointW[1]-ty) + (srz*sry + crz*srx*cry)*(pointW[2]-tz) + xn;
xnAve += xn;
ynAve += yn;
znAve += zn;
double x1 = crz * point[0] - srz * point[1];
double y1 = srz * point[0] + crz * point[1];
double z1 = point[2];
double x2 = x1;
double y2 = crx * y1 - srx * z1;
double z2 = srx * y1 + crx * z1;
pointE[0] += cry * x2 + sry * z2 + tx;
pointE[1] += y2 + ty;
pointE[2] += -sry * x2 + cry * z2 + tz;
}
xnAve /= poseNum[ir];
ynAve /= poseNum[ir];
znAve /= poseNum[ir];
pointE[0] /= poseNum[ir];
pointE[1] /= poseNum[ir];
pointE[2] /= poseNum[ir];
cout << "point estimate: " << pointE.transpose() << endl;
cout << "pose Num is: " << poseNum[ir] << endl;
cout << "Add noise rmse: " << sqrt(pow(xnAve, 2) + pow(ynAve, 2) + pow(znAve, 2)) << endl;;
cout << "estimate error rmse: " << sqrt(pow(pointE[0]-pointW[0],2) + pow(pointE[1]-pointW[1],2) + pow(pointE[2]-pointW[2],2)) << endl;
cout << "************iret: " << ir << " end*****************" << endl;
}
}
//g++ tri2_ave.cpp `pkg-config eigen3 --libs --cflags`
参考链接
三角测距方法：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
844
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
