ceres_solver解PnP平差问题、Matlab数据制作

最新推荐文章于 2024-10-17 13:38:31 发布

LiChengwei47

最新推荐文章于 2024-10-17 13:38:31 发布

阅读量2.8k

点赞数 1

分类专栏： SLAM_CV 文章标签： opencv 计算机视觉 slam pnp c++

本文链接：https://blog.youkuaiyun.com/it_LiChengwei/article/details/121455836

版权

网上的PnP平差没有数据，很痛苦，自己制作数据来解
根据其次坐标的推导公式
[u;v ]
在Matlab中制作数据(矩阵运算比较方便_{_})

clc
clear
X = zeros(15, 3);
x = zeros(15, 2);
K=[25 0 0;0 25 0;0 0 1;]%相机内参数矩阵
R=[0.4770710827172032, -0.7476726229304006, -0.4619402893831741;%旋转矩阵
 0.653281340399395, 0.653280157390419, -0.3826859368279936;
 0.5879002230999792, -0.1192085772095462, 0.8001016452918156];
%T表示投影矩阵P的第四列前三行
P4=[31.36648617639075;
 -17.35639716512237;
 -40.18578173498011];
T=inv(R)*P4;%[u;v]=K*R*[X;Y;Z]+T
%以下为带入一组数据验证公式的正确性
%A=K*(R*([50, 50, 75]')+P4);
%A=A/A(3)
for i = 1:15
    
    px = (101-99)*rand() + 99;
    py = 2*rand() - 1;
    pz = 2*rand() + 2;
    X(i,1)=px;
    X(i,2)=py;
    X(i,3)=pz;
    m = K*R* ([px;py;pz]-P4);
    u = m(1) / m(3);
    v = m(2) / m(3);
    x(i,1) = u;
    x(i,2) = v;
    
end
XT=X;
xt=x';
%一下为一种将数据保存.txt格式的方法
%      dlmwrite('myFile.txt',X,'delimiter',',')
%      type('myFile.txt')
%       dlmwrite('myFile2.txt',x,'delimiter',',')
%      type('myFile2.txt')
fileID1=fopen('X.txt','w');
fprintf(fileID1,'corners3D.push_back(Point3f(%1f, %1f, %1f));\n',XT);
fclose(fileID1);
type('X.txt')
fileID2=fopen('x2.txt','w');
fprintf(fileID2,'corners2D.pus