# 三角测量的一些基础理论

#### 构建超定方程组

T n 1 P ‾ − x T n 3 P ‾ = 0 T_{n1}\overline{P}-xT_{n3}\overline{P}=0 T n 2 P ‾ − y T n 3 P ‾ = 0 T_{n2}\overline{P}-yT_{n3}\overline{P}=0 其中 T n i T_{ni} 代表T矩阵的第i行。

#### 解的分析

#include <iostream>
#include <vector>
#include <random>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <fstream>

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 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);
// 绕 z轴 旋转
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());
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);
std::cout << tx << " " << ty << " " << tz << std::endl;

std::default_random_engine noise_generator;

double variance;
variance = 0.0;

std::normal_distribution<double> noise_pdf(0, variance);
double noise;

Eigen::Vector3d Pw(tx, ty, tz);
std::cout << "generate 3D point finished" << std::endl;

int start_frame_id = 0;
int end_frame_id = 1;

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);

noise = noise_pdf(noise_generator);
std::cout << noise << std::endl;
double x = Pc.x() + noise;
noise = noise_pdf(noise_generator);
std::cout << noise << std::endl;
double y = Pc.y() + noise;
noise = noise_pdf(noise_generator);
std::cout << noise << std::endl << std::endl;
double z = Pc.z() + noise;

camera_pose[i].uv = Eigen::Vector2d(x/z,y/z);
}
std::cout << "measurement generated" << std::endl;

Eigen::Vector3d P_est;
int D_row = 2 * (end_frame_id - start_frame_id);
Eigen::MatrixXd D;
D.resize(D_row, 4);
for(int i = 0; i < D_row/2; i++)
{
//对应camera_pose[start_frame+i]
Eigen::MatrixXd T(3,4);
Eigen::Matrix3d rotation = camera_pose[i+start_frame_id].Rwc.transpose();
Eigen::Vector3d trans = -1 * rotation * camera_pose[i+start_frame_id].twc;
for(int j = 0; j < 3; j++)
T.block(j, 0, 1, 4) << rotation(j,0), rotation(j,1), rotation(j,2), trans(j);
std::cout << "T:" << std::endl << T << std::endl;

D.block(i*2, 0, 1, 4) = camera_pose[i+start_frame_id].uv(0) * T.block(2, 0, 1, 4) - T.block(0, 0, 1, 4);
D.block(i*2+1,0,1, 4) = camera_pose[i+start_frame_id].uv(1) * T.block(2, 0, 1, 4) - T.block(1, 0, 1, 4);
}
std::cout << "D:" << std::endl << D << std::endl;
Eigen::MatrixXd square(4,4);
square = D.transpose() * D;

Eigen::JacobiSVD<Eigen::MatrixXd> svd(square, Eigen::ComputeThinU | Eigen::ComputeThinV);

std::cout << "Singular values: " << std::endl << svd.singularValues() << std::endl;
std::cout << "matrix U: " << std::endl << svd.matrixU() << std::endl;
std::cout << "matrix v: " << std::endl << svd.matrixV() << std::endl;
std::cout <<"ground truth: \n"<< Pw.transpose() <<std::endl;

double ddd = svd.matrixU()(1,3);
P_est << svd.matrixU()(0,3)/svd.matrixU()(3,3), svd.matrixU()(1,3)/svd.matrixU()(3,3), svd.matrixU()(2,3)/svd.matrixU()(3,3);
std::cout <<"my result: \n"<< P_est.transpose() <<std::endl;

return 0;
}


（如有需要转发，请注明出处～）

03-17 8231
08-15 1948

08-24 7644
02-21 760
03-03 5551
04-19 1314
08-04 1694
07-15 716
06-10 5163
09-22 1373
01-21 4137
01-30 3530