三⾓化特征点（triangulation ）⽅法及实现对⽐
⽂章⽬录
三⾓化特征点(triangulation)⽅法及实现对⽐
问题
已知两帧相机在世界坐标下的位姿, 求两帧的共视点的3维坐标。
设两帧为参考帧(reference)和当前帧(current)，共视点在两帧下的坐标分别为和，对应深度为和。图像归⼀化齐次坐标为和(⾮像素坐标)。
满⾜：
解决⽅法
1 构造AX=0的形式，⽤SVD 的⽅法进⾏求解
求解原理
[R ,t ]p x r x c d r d c p r p c p =r , p =⎣⎡u r v r 1⎦⎤c ⎣⎡u c v c 1
⎦⎤
x =r R x +rc c t rc
d p =r r d R p +c rc c t rc
设第帧相机的变换矩阵为。
则：
空间点的世界齐次坐标为，为观测点对应的深度值，为第帧相机该像素点的归⼀化齐次坐标。
得： （*）
由上述⽅程第三⾏可知：表⽰第三⾏。
将其带⼊(*)前两⾏，消去，得：
中秋节诗句
每次观测得到以上两个⽅程，经过多次观测，得到以下⽅程，将未知量移⾄⼀侧。
两次以上观测便可求解。
求解该问题的的⽅法：
由于未知量为齐次坐标，因此与(为常数)，所代表的坐标相同，因此设。
k T =k [R ,t ]∈k k R 3×4d p =k T x k w
x w d k p p k d =k ⎣⎡u k v k 1⎦⎤T k ⎣⎢⎢⎡x w y w z w 1
⎦⎥⎥⎤T k ,3T k d =k T x k ,3w
d k u T x =k k ,3w T x k ,1w
v T x =k k ,3w T x k ,2w
x w x =⎣⎢⎢⎢⎢⎢⎡u T −T 11,31,1v T −T 11,31,2⋮u T −T n n ,3n ,1v T −T n n ,3n ,2⎦⎥⎥⎥⎥⎥⎤w 0→A x =0
x w A x =0x =w [x ,y ,z ,1]w w w T x w c x w c ∣∣x ∣∣=21
求解问题变为：
拉格朗⽇乘⼦⽅程:
我们分别对和求偏导数：
整理得：
由上式可知：为的特征向量，且模长为1。
将进⾏SVD分解：
可知：为特征向量
从⽽可知，当值为的特征向量，的值为对应的特征值。
因此，问题的解值为对应的的特征向量。
在使⽤Eigen等库调⽤SVD接⼝时，⼀般会将奇异值按照从⼤到⼩的顺序排列，因此问题的解为的最后⼀列。解的有效性条件，对应的⾮常接近于0。
棋类大全
1）直接对A进⾏SVD分解，奇异值最⼩对应的V的列为解值。
2）对A^TA进⾏SVD分解(相当于三⾓化)，最⼩特征值对应的特征向量为解值。
代码实现
L (x ,λ)=x A Ax +T T λ(1−x x )
T x λ=∂x ∂(x ,λ)
2A Ax −T 2λx =0
=∂λ∂(x ,λ)
1−x x =T 0
A Ax =T λx , x x =T 1
x A A T A A =U ΣV , A A =T T V ΣU U ΣV =T T T V ΣΣV T T
V A A T ∣∣Ax ∣∣=2x A Ax =T T x λx =T λx x =T λ
x A A T ∣∣Ax ∣∣2∣∣Ax ∣∣=2λmin x λmin A A T x V λmin
auto loop_times = camera_po.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_po[j].anspo();
T_tmp.block<3,1>(0,3)=-camera_po[j].anspo()* camera_po[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_po[j].uv[0]* P_k3 - P_k1;
A.block<1,4>(2* j +1,0)= camera_po[j].uv[1]* P_k3 - P_k2;
}
//solution1: SVD1
Matrix4d ATA = A.transpo()* 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 poRand(time(NULL));
/
/poRand.ed(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>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:"<< anspo()<< endl;
int irNum =7;
int poNum[irNum]={2,10,20,100,200,500,1000};
for(int ir=0; ir < irNum; ir++){
Eigen::MatrixXd mapPointA(2*poNum[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 < poNum[ir]; i++){
rx +=rxr(poRand);
ry +=ryr(poRand);
rz +=rzr(poRand);
tx +=txr(poRand);
ty +=tyr(poRand);
tz +=tzr(poRand);
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(poRand);
point[0]=(crz*cry + srz*srx*sry)*pointW[0]+ srz*crx*pointW[1]+(-crz*sry + srz*srx*cry)*pointW[2]- tx +pr(poRand); point[1]=(-srz*cry + crz*srx*sry)*pointW[0]+ crz*crx*pointW[1]+(srz*sry + crz*srx*cry)*pointW[2]- ty +pr(poRand);
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 = anspo()*mapPointA;
Eigen::JacobiSVD<Eigen::MatrixXd>svd(ATA,
Eigen::ComputeFullU|Eigen::ComputeFullV);
auto res_U = svd.matrixU();
auto lambda = svd.singularValues();
