2完成三角化代码
源码如下：
//
// Created by nnz on 18-11-11.
//
#include <iostream>
#include <vector>
#include <random>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
using namespace std;
//
struct Pose
{
Pose(Eigen::Matrix3d R, Eigen::Vector3d t):Rwc(R),qwc(R),twc(t) {
};
Eigen::Matrix3d Rwc;
Eigen::Quaterniond qwc;
Eigen::Vector3d twc;
Eigen::Vector2d uv; // 这帧图像观测到的特征坐标
};
int main()
{
int poseNums = 10;
double radius = 8;//半径
double fx = 1.;
double fy = 1.;
std::vector<Pose> camera_pose;//
for(int n = 0; n < poseNums; ++n )//生成的
{
double theta = n * 2 * M_PI / ( poseNums * 4); // 1/4 圆弧
// 绕 z轴 旋转
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());
Eigen::Vector3d t = Eigen::Vector3d(radius * cos(theta) - radius, radius * sin(theta), 1 * sin(2 * theta));
camera_pose.push_back(Pose(R,t));
}
// 随机数生成 1 个 三维特征点
std::default_random_engine generator;
std::uniform_real_distribution<double> xy_rand(-4, 4.0);
std::uniform_real_distribution<double> z_rand(8., 10.);
double tx = xy_rand(generator);
double ty = xy_rand(generator);
double tz = z_rand(generator);
Eigen::Vector3d Pw(tx, ty, tz);
// 这个特征从第三帧相机开始被观测，i=3
int start_frame_id = 3;
int end_frame_id = poseNums;
for (int i = start_frame_id; i < end_frame_id; ++i) {
Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();
Eigen::Vector3d Pc = Rcw * (Pw - camera_pose[i].twc);
double x = Pc.x();
double y = Pc.y();
double z = Pc.z();
camera_pose[i].uv = Eigen::Vector2d(x/z,y/z);
}
/// TODO::homework; 请完成三角化估计深度的代码
// 遍历所有的观测数据，并三角化
Eigen::Vector3d P_est; // 结果保存到这个变量
P_est.setZero();
/* your code begin */
// step 1 construct D
int D_rows=2*(end_frame_id-start_frame_id);//D的行数
Eigen::MatrixXd D=Eigen::MatrixXd::Zero(D_rows,4);//初始化一下D
for(int i=start_frame_id;i<end_frame_id;++i)
{
Eigen::Matrix<double,3,4> Tcw;
Eigen::Matrix3d Rcw=camera_pose[i].Rwc.transpose();
Eigen::Vector3d tcw=-Rcw*camera_pose[i].twc;
Tcw<<Rcw,tcw;
D.block(2*(i-start_frame_id),0,1,4)=camera_pose[i].uv(0)*Tcw.row(2)
-Tcw.row(0);
D.block(2*(i-start_frame_id)+1,0,1,4)=camera_pose[i].uv(1)*Tcw.row(2)
-Tcw.row(1);
}
//step 2 放缩 max取D的最大的元素
double max=D.maxCoeff();
cout<<"D = "<< endl<<D <<endl;
cout<<"The max num of D ="<< max <<endl;
Eigen::MatrixXd DTD=(D/max).transpose()*(D/max);
clock_t time_stt = clock();
Eigen::JacobiSVD<Eigen::MatrixXd> svd_h(DTD,Eigen::ComputeThinU | Eigen::ComputeThinV);
clock_t time_stt1 = clock();
std::cout<<"SVD分解，耗时:\n"<<(clock()-time_stt1)/(double) CLOCKS_PER_SEC<<"ms"<<std::endl;
Eigen::MatrixXd U=svd_h.matrixU();
Eigen::MatrixXd V=svd_h.matrixV();
Eigen::MatrixXd S(svd_h.singularValues());
cout<<"U = "<<endl<<U<<endl;
cout<<"V = "<<endl<<V<<endl;
cout<<" singularValues "<< endl<<S <<endl;
//step 3 判断是否有效
if(abs(svd_h.singularValues()[3]/svd_h.singularValues()[2])<1e-2)
{
Eigen::Vector4d u4=max*U.block(0,3,4,1);
P_est=(u4/u4[3]).head(3);//归一化
}
else
{
cout<<"无效!"<<endl;
}
/* your code end */
std::cout <<"ground truth: \n"<< Pw.transpose() <<std::endl;
std::cout <<"your result: \n"<< P_est.transpose() <<std::endl;
return 0;
}
运行结果如下：
D =
-0.891007 -0.45399 -0.478253 1.25886
0.45399 -0.891007 -0.339294 3.90642
-0.809017 -0.587785 -0.548566 2.04958
0.587785 -0.809017 -0.432396 5.11352
-0.707107 -0.707107 -0.626707 2.96985
0.707107 -0.707107 -0.51176 6.16861
-0.587785 -0.809017 -0.707633 3.97072
0.809017 -0.587785 -0.571913 7.01606
-0.45399 -0.891007 -0.78662 5.00446
0.891007 -0.45399 -0.609762 7.62136
-0.309017 -0.951057 -0.860286 6.03353
0.951057 -0.309017 -0.625116 7.97589
-0.156434 -0.987688 -0.927108 7.03502
0.987688 -0.156434 -0.620237 8.09317
The max num of D =8.09317
SVD分解，耗时:
1e-06ms
U =
0.0530721 0.846878 0.41558 0.327528
-0.103079 0.431629 -0.895388 0.0367562
-0.102585 0.309021 0.122288 -0.937565
0.987945 0.0316285 -0.103049 -0.111113
V =
0.0530721 0.846878 0.41558 0.327528
-0.103079 0.431629 -0.895388 0.0367562
-0.102585 0.309021 0.122288 -0.937565
0.987945 0.0316285 -0.103049 -0.111113
singularValues
7.1513
0.118267
0.0110422
1.88518e-17
ground truth:
-2.9477 -0.330799 8.43792
your result:
-2.9477 -0.330799 8.43792