%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%555
%%%%对两帧图像进行ICM调试
%%%计算机视觉中的摄像机标定方法[1].pdf
%%空间机器人中相机标定方法探讨.pdf
clc; clear;
%path = 'D:\视频数字特效\姿态跟踪\ICM-sift3(取第一帧和上帧平均)\wsg视频截图\';
%path='F:\matlabProject\结题——视频跟踪20080806\结题——视频跟踪_080806\ICM-sift\wsg视频截图\wsg视频截图\'
path='E:\3DFaceTrackingPic\TEST1\';
file1 = '1.jpg';
file2 = '2.jpg';
wsg1 = imread([path,file1]);%%假设为第一帧
wsg2 = imread([path,file2]);%%假设为第二帧
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%对第一帧估计姿态和摄像机内参数
% feature = [216,280;226,275;235,282;226,284;
% 266,282;274,277;283,282;274,285;
% 234,308;248,304;261,306;
% 231,331;249,323;266,331;249,338;
% 197,294;200,321;209,347;249,363;283,348;293,323;296,297];
% feature2D = [feature(:,1)-(256-159-1)*ones(size(feature,1),1),feature(:,2)-(256-119-1)*ones(size(feature,1),1)];
% % feature2D = feature2Dy - [feature2Dy(10,1)*ones(size(feature2Dy,1),1),feature2Dy(10,2)*ones(size(feature2Dy,1),1)];
% feature2D = [119,144;124,141;131,140;136,142;140,147;135,147;130,147;123,147;
% 168,146;171,143;177,142;183,143;188,146;182,148;177,149;173,148;
% 147,144;143,154;140,162;137,170;142,177;148,178;
% 156,178;163,178;168,173;161,163;161,156;160,145;
% 133,195;139,191;146,187;153,188;162,187;167,189;172,195;167,198;162,201;153,203;147,201;139,201;
% 139,195;147,195;153,194;161,195;167,194;
% 101,145;102,159;103,170;104,184;108,199;115,211;128,221;153,226;177,218;185,213;192,203;196,186;199,171;200,158;200,146];
load feature_point
for kkk=1:22
for kkkk=1:2
feature2D(kkk,kkkk)=feature_point(1,(kkk-1)*2+kkkk);
end
end
feature3D = [129;420;9604;9208;
30112;21030;20741;29716;
14859;35554;35264;
20215;11819;40824;40099;
1281;5320;19033;37270; 39438;35625;22094];
% 177;13097;19033;37270; 39438; 33503;20785];
% 177;13097;19487;37270; 39537; 33503;20785];
% feature3D = [138;432;427;220;9407;9208;9112;9522;
% 30109;21025;21130;21137;20740;30226;30018;29912;
% 9500;7986;6371;14759;13341;13133;
% 33636;33843;35564;27074;28488;30104;
% 20216;10617;11319;11516;31922;31220;40819;40613;40406;19697;19802;19908;20313;20408;10304;30912;41017;
% 771;9857;6524;13087;10557;19333;18506;17476;38702;39931;32164;35097;28130;30453;21065];
fid = fopen('Shape_new.txt','rt');
Shape = fscanf(fid,'%i %f %f %f\n');
fclose(fid);
Shape = reshape(Shape,4,size(Shape,1)/4)';
tmp = Shape(feature3D(22),2)*ones(size(Shape,1),1);
tmp = [tmp,Shape(feature3D(22),3)*ones(size(Shape,1),1)];
tmp = [tmp,Shape(feature3D(22),4)*ones(size(Shape,1),1)];
Shape(:,2:4) = Shape(:,2:4) - tmp;
feature3D_coor = zeros(size(feature3D,1),3);
for m = 1:size(feature3D,1)
feature3D_coor(m,:) = Shape(feature3D(m),2:4);
end
Coe_matrix = zeros(2*size(feature3D,1),11);
for m=1:size(feature3D,1)
Coe_matrix(2*m-1,1:4) = [Shape(feature3D(m),2:4),1];
Coe_matrix(2*m,5:8) = [Shape(feature3D(m),2:4),1];
Coe_matrix(2*m-1,9:11) = -1.*Shape(feature3D(m),2:4).*feature2D(m,1);
Coe_matrix(2*m,9:11) = -1.*Shape(feature3D(m),2:4).*feature2D(m,2);
end
b = reshape(feature2D',2*size(feature2D,1),1);
m_para = pinv(Coe_matrix'*Coe_matrix)*Coe_matrix'*b;
m_para = [m_para(1:4);m_para(9:11);m_para(5:8)];
tz = sqrt(1/(m_para(5)^2+m_para(6)^2+m_para(7)^2));
u0 = tz^2*(m_para(1)*m_para(5)+m_para(2)*m_para(6)+m_para(3)*m_para(7));
kx = sqrt(tz^2*(m_para(1)*m_para(1)+m_para(2)*m_para(2)+m_para(3)*m_para(3))-u0*u0);
r7 = m_para(5)*tz;
r8 = m_para(6)*tz;
r9 = m_para(7)*tz;
r1 = (m_para(1)*tz-u0*r7)/kx;
r2 = (m_para(2)*tz-u0*r8)/kx;
r3 = (m_para(3)*tz-u0*r9)/kx;
% r4 = r3*r8-r2*r9;
% r5 = r1*r9-r3*r7;
% r6 = r2*r7-r1*r8;
v0 = tz^2*(m_para(8)*m_para(5)+m_para(9)*m_para(6)+m_para(10)*m_para(7));
ky = sqrt(tz^2*(m_para(8)*m_para(8)+m_para(9)*m_para(9)+m_para(10)*m_para(10))-v0*v0);
r4 = (m_para(8)*tz-v0*r7)/ky;
r5 = (m_para(9)*tz-v0*r8)/ky;
r6 = (m_para(10)*tz-v0*r9)/ky;
tx = (m_para(4)*tz-u0*tz)/kx;
ty = (m_para(11)*tz-v0*tz)/ky;
% f = kx/sx;
Rot = [r1,r2,r3;r4,r5,r6;r7,r8,r9]; %摄像机外参数
Tran = [tx;ty;tz];
% for m=1:size(feature3D,1)%Shape
% XYZw = Shape(feature3D(m),2:4)';
% XYZc = Rot*XYZw+Tran;
% Xf = u0 + kx*(XYZc(1)/XYZc(3));
% Yf = v0 + ky*(XYZc(2)/XYZc(3));
% wsg(round(Yf),round(Xf),:)=[0,0,0];
% wsg(round(Yf)-2:round(Yf)+2,round(Xf)-2:round(Xf)+2,:)=zeros(5,5,3);
% end
% figure;imshow(wsg);
tmp = wsg1;
val_point=1;
for m=1:size(Shape,1)%feature3Dfeature3D
XYZw = Shape((m),2:4)';
XYZc = Rot*XYZw+Tran;
Xf = u0 + kx*(XYZc(1)/XYZc(3));
Yf = v0 + ky*(XYZc(2)/XYZc(3));
if(find(feature3D==m)~=0)
tmp(round(Yf),round(Xf),:)=[0,0,0];
wsg1(val_point,1)=round(Yf);
wsg1(val_point,2)=round(Xf);
val_point=val_point+1;
end
%tmp(round(Yf),round(Xf),:) = [0,0,0];
% wsg(round(Yf)-2:round(Yf)+2,round(Xf)-2:round(Xf)+2,:) = zeros(5,5,3);+feature2Dy(10,1)+feature2Dy(10,2)
end
figure;imshow(tmp);
% fid = fopen('triangle2.txt','rt');
% triangle = fscanf(fid,'%i %i %i %i\n');
% fclose(fid);
% triangle = reshape(triangle,4,size(triangle,1)/4)';
% %%%%end对第一帧估计姿态和摄像机内参数
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%采用块匹配法对第二帧图像的特征点进行初始估计
%%%对两幅图做四次金字塔
GaussPyramid_a = 0.375;
GaussPyramid = [0.25-GaussPyramid_a/2,0.25,GaussPyramid_a,0.25,0.25-GaussPyramid_a/2]';
feature3D_coor_new_orig = zeros(size(feature3D_coor));
for m=1:size(feature3D_coor,1)
XYZw = feature3D_coor(m,:)';
XYZw = Rot*XYZw + Tran;
feature3D_coor_new_orig(m,:) = XYZw';
end
Shape_New = zeros(size(Shape));
Shape_New(:,1) = Shape(:,1);
tmp = Shape(:,2:4)';
tmp = Rot*tmp + Tran*ones(1,size(Shape,1));
Shape_New(:,2:4) = tmp';
Shape_New_first = Shape_New;
for fig_num=1:100
file2 = num2str(fig_num);
file2 = [file2,'.jpg'];
wsg2 = imread([path,file2]);
file1 = num2str(1);
file1 = [file1,'.jpg'];
wsg1_first = imread([path,file1]);
if fig_num == 1
file1 = '1.jpg';
wsg1 = imread([path,file1]);%%假设为第一帧为关键帧
Shape_New = zeros(size(Shape));
Shape_New(:,1) = Shape(:,1);
tmp = Shape(:,2:4)';
tmp = Rot*tmp + Tran*ones(1,size(Shape,1));
Shape_New(:,2:4) = tmp';
else
file1 = num2str(fig_num-1);
file1 = [file1,'.bmp'];
wsg1 = imread([path,file1]);
end
%%产生灰度图
wsg1_gray = rgb2gray(wsg1);
wsg2_gray = rgb2gray(wsg2);
% err_iter = 100;
% while 1
%
% Shape_New_last = Shape_New;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%sift特征匹配
matchind = 0;des1= 0; loc1= 0;des2= 0; loc2= 0;
[num,matchind,des1, loc1,des2, loc2] = match(wsg1_gray, wsg2_gray);
feature2D_sift_imag1 = zeros(num,2);
feature2D_sift_imag2 = zeros(num,2);
flag = 0;
for n = 1:length(matchind)
if (matchind(n) > 0)
Y = (loc1((n),1));
X = (loc1((n),2));
Y2 = (loc2(matchind(n),1));
X2 = (loc2(matchind(n),2));
flag = flag +1;
feature2D_sift_imag1(flag,:) = [X,Y];
feature2D_sift_imag2(flag,:) = [X2,Y2];
end
end
%%三维坐标到图像坐标系
ShapeToImage = Shape_New(:,2:4);
ShapeToImage = [kx.*ShapeToImage(:,1)./ShapeToImage(:,3),ky.*ShapeToImage(:,2)./ShapeToImage(:,3)];
ShapeToImage = ShapeToImage + ([u0,0;0,v0]*ones(2,size(Shape,1)))';%focalLength*
% tmp = wsg1_gray;
% for m=1:size(Shape_New,1)%feature3Dfeature3D
% XYZc = Shape_New((m),2:4)';
% Xf = u0 + kx*(XYZc(1)/XYZc(3));
% Yf = v0 + ky*(XYZc(2)/XYZ
评论0