1
2
code show as below:
//
// Created by hyj on 18-11-11.
//
#include <iostream>
#include <vector>
#include <random>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
struct Pose //位姿结构体
{
Pose(Eigen::Matrix3d R, Eigen::Vector3d t):Rwc(R),qwc(R),twc(t) {
};//有参构造初始化pose
Eigen::Matrix3d Rwc;//旋转矩阵
Eigen::Quaterniond qwc;//四元数
Eigen::Vector3d twc;//位置/平移
};
int main()
{
int featureNums = 20;//特征路标点个数
int poseNums = 10;//相机个数
int diem = poseNums * 6 + featureNums * 3;//信息矩阵/H矩阵维数
double fx = 1.;//焦距
double fy = 1.;//
Eigen::MatrixXd H(diem,diem);
H.setZero();//H矩阵初始化
//生成各个相机位姿
std::vector<Pose> camera_pose;//容器 camera_pose存放各个相机位姿
double radius = 8;//圆弧半径
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));//自定义R t
}
// 随机数生成三维特征点
std::default_random_engine generator;//随机数产生器
std::vector<Eigen::Vector3d> points;//容器points存放三维特征点
for(int j = 0; j < featureNums; ++j)
{
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);//世界坐标
points.push_back(Pw);
//遍历相机
for (int i = 0; i < poseNums; ++i) {
//得到从世界到相机的位姿
Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();
Eigen::Vector3d Pc = Rcw * (Pw - camera_pose[i].twc);//相机下的三维点
// 这里用了Pw - camera_pose[i].twc 我怎么感觉应该是Pw + camera_pose[i].twc (如果camera_pose[i].twc是相机到世界的平移的话)
double x = Pc.x();
double y = Pc.y();
double z = Pc.z();
double z_2 = z * z;
Eigen::Matrix<double,2,3> jacobian_uv_Pc;//这里的jacobian_uv_Pc雅克比矩阵是
jacobian_uv_Pc<< fx/z, 0 , -x * fx/z_2,
0, fy/z, -y * fy/z_2;
Eigen::Matrix<double,2,3> jacobian_Pj = jacobian_uv_Pc * Rcw;
Eigen::Matrix<double,2,6> jacobian_Ti;//这里的jacobian_Ti雅克比矩阵和slambook2的p187页的公式7.46的顺序不太一样
jacobian_Ti << -x* y * fx/z_2, (1+ x*x/z_2)*fx, -y/z*fx, fx/z, 0 , -x * fx/z_2,
-(1+y*y/z_2)*fy, x*y/z_2 * fy, x/z * fy, 0,fy/z, -y * fy/z_2;
//这里指的是左上角的相机与相机构成的h/信息矩阵
H.block(i*6,i*6,6,6) += jacobian_Ti.transpose() * jacobian_Ti;
/// 请补充完整作业信息矩阵块的计算
//这里指的是右下角的路标点与路标点构成的h/信息矩阵
H.block(j*3 + 6*poseNums,j*3 + 6*poseNums,3,3) +=jacobian_Pj.transpose()*jacobian_Pj;
//这里指的是左下角相机与路标点构成的h/信息矩阵
H.block(i*6,j*3 + 6*poseNums, 6,3) += jacobian_Ti.transpose()*jacobian_Pj;
//这里指的是右上角的路标点和相机构成的h/信息矩阵
H.block(j*3 + 6*poseNums,i*6 , 3,6) += jacobian_Pj.transpose() * jacobian_Ti;
}
}
// std::cout << H << std::endl;
// Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(H);
// std::cout << saes.eigenvalues() <<std::endl;
Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
std::cout << svd.singularValues() <<std::endl;
return 0;
}
The most critical code is:
//这里指的是左上角的相机与相机构成的h/信息矩阵
H.block(i*6,i*6,6,6) += jacobian_Ti.transpose() * jacobian_Ti;
/// 请补充完整作业信息矩阵块的计算
//这里指的是右下角的路标点与路标点构成的h/信息矩阵
H.block(j*3 + 6*poseNums,j*3 + 6*poseNums,3,3) +=jacobian_Pj.transpose()*jacobian_Pj;
//这里指的是左下角相机与路标点构成的h/信息矩阵
H.block(i*6,j*3 + 6*poseNums, 6,3) += jacobian_Ti.transpose()*jacobian_Pj;
//这里指的是右上角的路标点和相机构成的h/信息矩阵
H.block(j*3 + 6*poseNums,i*6 , 3,6) += jacobian_Pj.transpose() * jacobian_Ti;
The results of the operation are as follows:
147.51
129.146
108.42
86.8786
66.7165
53.3602
50.0706
49.9072
46.1212
41.646
37.4764
36.9655
32.5123
29.4255
28.6926
25.7302
25.2799
23.9525
23.4874
22.1444
3.75546
3.63333
3.59899
3.48363
3.44025
3.26522
3.2201
2.97983
2.74213
2.53162
0.420392
0.394421
0.349009
0.304702
0.284198
0.269621
0.261614
0.253395
0.250928
0.247525
0.233361
0.20687
0.195922
0.185235
0.182312
0.180179
0.17752
0.174138
0.16958
0.168057
0.163943
0.161402
0.158017
0.154701
0.152969
0.146307
0.145153
0.144042
0.141768
0.135736
0.131691
0.126308
0.122724
0.122139
0.120157
0.119543
0.116115
0.0479554
0.0391613
0.0382714
0.0370505
0.0348714
0.0338867
0.0307974
0.0285107
0.0273249
0.0256439
0.024074
0.0217149
0.0189657
0.0180311
0.0160844
0.0156509
0.0143411
0.0136067
0.0133183
0.0130406
0.0128135
0.0125244
0.0122748
0.0104077
0.00982936
0.00897393
0.0082873
0.00763381
0.00734249
0.00701361
0.00634341
0.00608493
0.00547299
0.0053236
0.00520788
0.00502341
0.0048434
0.00451083
0.0042627
0.00386223
0.00351651
0.00302963
0.00253459
0.00230246
0.00172459
0.000422374
3.21708e-17
2.06732e-17
1.43188e-17
7.66992e-18
6.08423e-18
6.05715e-18
3.94363e-18