Commit f0154c21 authored by paulgay's avatar paulgay
Browse files

adding comment on em_orient

parent 1834bd4b
function [C,Eh,scales,axes_red,Sigma_noise]=em(prs,classes,measure,C,m_values,angles,Gs)
mm=mean(measure(:));
measure=measure/mm;
C=C/mm;
defaultNoise=0;
if defaultNoise~=0
fprintf('default noise modified %d\n',defaultNoise)
end
Nmax=10;%number max of iterations
function [C,Eh,scales,axes_red,Sigma_noise,out]=em(prs,classes,measure,C,m_values,Gs,inc,defaultNoise,Nmax)
%%%%
% Compute the expectations of the axis length, orientation.
% inputs:
% prs: the prior distribution parameters
% classes: the object labels
% measure: the observed conics
% C: the initial sfmo camera
% m_values: missing data pattern, it is a boolean matrix
% Gs: the ellipsoid version of the previous
% inc: contains various variable usefull for debugging and inspecting the performances on a per iteration basis
% defaultNoise: initialisation of the noise covariance matrix. If set to 0, it will default to 1 and will be estimated if not, it will be kept as it is.
% Nmax: the maximum number of iterations. With the use of the MCMC, the em algorithm is not guaranted to converge. If not provided, it will default to 1, i.e. only one Estep is done. Note that if the defaultNoise is different than 0
% outputs:
% C: the camera matrix. In this version, it is left unchanged.
% Eh: the expectation of the latent variables: here these are the dual centred quadrics as a 6 dim vector.
% scales, axe_red, : the normalise axis length and their scale
% Sigma_noise: the noise covariance matrix
% out: Will contain the estimated quadric with the 6 different possible correspondences between the quadric and the prior, as mentioned in the end of Sec 2.3 of the PSFMO ICCV 2017 pqper
if nargin < 10
Nmax = 1 % max number of iterations
end
[n_images3,n_obj]=size(measure);
Sigma_noise=eye(n_images3);
if defaultNoise~=0
Sigma_noise=Sigma_noise*defaultNoise;
else
Sigma_noise=Sigma_noise*1000;
end
Lold=-Inf;
fprintf('initialising the covariance matrix to: %d\n',defaultNoise)
Lold=-Inf;% will store the likelihood
passed=0;
Cold=C;
Sigma_noiseold=Sigma_noise;
......@@ -24,7 +43,8 @@ scales=zeros(n_obj,1);
axes=zeros(3,n_obj);
angles_pre=angles;
for n=1:Nmax
[Eh,Ehh,scales_post,axes_post,axes_red,angles_post,~]=h_exp_first(prs,classes,measure,C,Sigma_noise,axes,scales,angles_pre,Gs);
% estep
[Eh,Ehh,scales_post,axes_post,axes_red,angles_post,~,out]=h_exp_first(prs,classes,measure,C,Sigma_noise,axes,scales,angles_pre,Gs,inc);
if sum(isnan(Eh(:)))>0
disp('nan rate')
sum(isnan(Eh(:)))/numel(Eh)
......@@ -93,7 +113,7 @@ end
end
function [Eh,Ehh,scales,axes_post,axes_red,angles_post,chs]=h_exp_first(prs,classes,measure,C,Sigma_noise,axes_post,scales,angles_post,Gs)
function [Eh,Ehh,scales,axes_post,axes_red,angles_post,chs,out]=h_exp_first(prs,classes,measure,C,Sigma_noise,axes_post,scales,angles_post,Gs,inc)
%computing posterior variance for each object.
[~,n_obj]=size(measure);
prec_data=inv(Sigma_noise);
......@@ -104,20 +124,13 @@ lastwarn('');
chs=zeros(n_obj,3);
axes_red=zeros(2,n_obj);
for i=1:n_obj
% [~, msgid]=lastwarn;
% if strcmp(msgid,'MATLAB:nearlySingularMatrix') || strcmp(msgid,'MATLAB:singularMatrix')
% Eh(:,i)=NaN;
% Ehh(:,:,i)=NaN;
% continue
% end
% lastwarn('');
sfmo_axes=[ Gs{i}.e0 Gs{i}.e1 Gs{i}.e2]';
%for k=1:20
init_angle=angles_post(:,i);
R=rot_euler2rot(init_angle)';
Rm=[ R(1,:).^2 ; R(1,:).*R(2,:) ; R(1,:).*R(3,:); R(2,:).^2; R(2,:).*R(3,:); R(3,:).^2];
perm = perms(1:3);
mu_axes =prs.means(:,classes(i));
prec_axes=inv(prs.vars(:,classes(i)*2-1:classes(i)*2));
sfmo_axes=[ Gs{i}.e0 Gs{i}.e1 Gs{i}.e2]';
gmm=prs.gmms(classes(i)).gmm;
sc=norm(sfmo_axes);
for cc=1:size(perm,1)
ch=perm(cc,:);
......@@ -127,14 +140,24 @@ for i=1:n_obj
l=l/sc;
h=P'*(l.^2-mu);
[~, msgid]=lastwarn;
logPfuns={@(m) log_hammer_obs(m,measure(:,i),prec_data,mu_axes,prec_axes,Rm,P,mu,C,0)};
logPfuns={@(m) log_hammer_obs2(m,measure(:,i),prec_data,gmm,Rm,P,mu,C,0)};
%init=[ linspace(h(1),0,8);linspace(h(2),0,8)]; minit=[ [0.1 1 10 100 0.1 1 10 100 ]*sc ; init];
minit=[ [0.1 1 10 100 0.1 1 10 100 ]*sc ;mvnrnd(repmat(h,1,8)',eye(2)*max(mean(prs.vars(:,classes(i)*2-1:classes(i)*2))))' ];
minit=[ [0.1 1 10 100 0.1 1 10 100 ]*sc ;mvnrnd(repmat(h,1,8)',eye(2))' ];
%minit=[ [0.1 1 10 100 0.1 1 10 100 ]*sc ;zeros(2,8) ];
%minit=[ [0.1 1 10 100 0.1 1 10 100 0.1 1 10 100 0.1 1 10 100 ]*1000 ;10*mvnrnd(repmat(mu_axes,1,16)',prs.vars(:,classes(i)*2-1:classes(i)*2))' ];
%minit=[ [0.1 1 10 100 0.1 1 10 100 ]*1000 ;mvnrnd(repmat(mu_axes,1,8)',prs.vars(:,classes(i)*2-1:classes(i)*2))' ];
[modi,lgP]=gwmcmc(minit,logPfuns,1000,'ThinChain',1);
models(cc*3-2:cc*3,:)=modi(:,:);
logP(cc,:)=lgP(:,:);
[~,ii]=max(logP(cc,:));
Ehi=models(cc*3-2:cc*3,ii);
axes_norm=P*Ehi(2:3)+mu;
axes_norm(logical([ 0; axes_norm(2:3)<0 ]) ) = 0.1;
l=(Ehi(1)*sqrt(axes_norm)).^2;
out.Ehi(i,:,cc)=Rm*l;
out.costg(i,cc)=log_hammer_obs2(Ehi,measure(:,i),prec_data,gmm,Rm,P,mu,C,0);
out.coste(i,cc)=norm(C*Rm*l-measure(:,i));
end
[~,r]=max(max(logP,[],2));
[~,index]=max(logP(r,:));
......@@ -142,18 +165,15 @@ for i=1:n_obj
chs(i,:)=perm(r,:);
ch=perm(r,:);
P=prs.ps(ch,classes(i)*2-1:classes(i)*2);
log_hammer_obs2(models(r*3-2:r*3,index),measure(:,i),prec_data,gmm,Rm,P,mu,C,1);
mu=prs.mus(ch,classes(i));
axes_red(:,i)=Ehi(2:3);
axes_norm=P*Ehi(2:3)+mu;
%l=Ehi(1)*axes_norm;
l=(Ehi(1)*sqrt(abs(axes_norm))).^2;
axes_norm=abs(P*Ehi(2:3)+mu);
l=(Ehi(1)*sqrt(axes_norm)).^2;
scales(i)=Ehi(1);
axes_post(:,i)=axes_norm;
obs=C*Rm*l;
x = lsqnonlin(@(x) cost(x,C,obs,l),init_angle);
if i==3
log_hammer_obs(Ehi,measure(:,i),prec_data,mu_axes,prec_axes,Rm,P,mu,C,1)
end
%x = lsqnonlin(@(x) cost(x,C,obs,l),init_angle);
x=init_angle;
angles_post(:,i)=x;
R=rot_euler2rot(x)';
......@@ -161,18 +181,94 @@ for i=1:n_obj
Ehi=Rm*l;
covi=cov((Rm* ( repmat(models(r*3-2,:),3,1).*(P*models(r*3-1:r*3,:))))');
Ehhi=Ehi*Ehi' + covi;
%end
Eh(:,i)=Ehi;
Ehh(:,:,i)=Ehhi;
if 0
Rec_post = recombine_ellipsoids(Eh,inc.S1);
%transforming the quadrics into ellipsoids.
qvecs_sfmo=zeros(10,n_obj);
qvecs_psfmo=qvecs_sfmo;
for o=1:length(inc.GT)
qvecs_sfmo(:,o)=sym2vec(inc.Rec{o}.Q);
qvecs_psfmo(:,o)=sym2vec(Rec_post{o}.Q);
end
Rec_new=quadric2ellipsoide(qvecs_sfmo);
Rec_post_new=quadric2ellipsoide(qvecs_psfmo);
for o=1:length(inc.GT)
Rec_new{o}.Q=ellipsoide2quadric(Rec_new{o});
Rec_post_new{o}.Q=ellipsoide2quadric(Rec_post_new{o});
end
x3cGT = compCent(inc.GT);
[dummy,x3c_transf,transproc] = procrustes(real(x3cGT)',real(inc.S1)');
[ RecT ] = tranEllipsoid( Rec_new, transproc );
[mea,oRec,AP3D,table3D] = results3D(RecT,inc.GT,50000);
[ Rec_postT ] = tranEllipsoid( Rec_post_new, transproc );
[meap,oRec,AP3Dp,table3D] = results3D(Rec_postT,inc.GT,50000);
[dummy,x3c_transf,transproc] = procrustes(real(inc.S1)',real(x3cGT)');
[ GTT ] = tranEllipsoid( inc.GT, transproc );
function r=log_hammer_obs(m,mu_data,prec_data,mu_axes,prec_axes,R,P,mu,C,v)
%axes=m(1)*(P*m(2:3)+mu);
if 0 %plotting
close all;
rec = plot3Dscene2(inc.GT, Rec_postT,x3cGT, n_obj, 0, [2 3 4], 9, 0, 0, [0 1 0],[0 0 1]);
[~] = plot3Dscene2(inc.GT, RecT, x3cGT, n_obj, 0, [], 9, 0, 0, [0 1 0],[1 0 0]);
rec = plot3Dscene2(inc.Rec, Rec_post,x3cGT, n_obj, 0, [], 9, 0, 0, [1 0 0],[0 0 1]);
%rec = plot3Dscene2(Rec_new, GTT, x3cGT, n_obj, 0, [], 9, 0, 0, [1 0 0],[0 0 1]);
hold on;
for f=1:length(inc.Imm)
A=inc.R1(2*f-1:2*f,:);
[V,l]=eig(A'*A);
[~,index]=min(diag(l));
line( [ 0 V(1,index) ], [ 0 V(2,index) ], [ 0 V(3,index) ]);
end
hold off;
Nf=length(inc.Imm);
%Crec = reprQ(inc.R1,inc.Rec,ones(Nf,n_obj),'ortho',1);
%Crec_post = reprQ(inc.R1,Rec_post,ones(Nf,n_obj),'ortho',1);
Crec = reprQ(inc.R1,Rec_new,ones(Nf,n_obj),'ortho',1);
%Crec_post = reprQ(inc.R1,GTT,ones(Nf,n_obj),'ortho',1);
Crec_post = reprQ(inc.R1,Rec_post_new,ones(Nf,n_obj),'ortho',1);
Crec = retrasl(Crec,inc.TT);
Crec_post = retrasl(Crec_post,inc.TT);
rowzero=[];
for f=1:max(1,round(length(inc.Imm)/5)):length(inc.Imm) %
if sum(rowzero==f)~=0
continue
end
figure;
imshow(inc.Imm{f}.I); hold on;
for o=1:(size(Crec_post,2)/3)
plotEllipse(inc.Cnotrasl(3*f-2:3*f,3*o-2:3*o)/(-inc.Cnotrasl(3*f,3*o)),[0 1 0],4); hold on;
plotEllipse(Crec(3*f-2:3*f,3*o-2:3*o)/(-Crec(3*f,3*o)),[1 0 0],4); hold on;
plotEllipse(Crec_post(3*f-2:3*f,3*o-2:3*o)/(-Crec_post(3*f,3*o)),[0 0 1],4); hold on;
end
end
end
[ AP3D AP3Dp]
end
a=1;
end
Sigma_noise(1);
b=2;
end
function r=log_hammer_obs2(m,mu_data,prec_data,gmm,R,P,mu,C,v)
axes=m(1)*sqrt(abs(P*m(2:3)+mu));axes=axes.^2;
obs_gen=C*R*axes;
first=((obs_gen-mu_data)'*prec_data*(obs_gen-mu_data))/length(mu_data);
sec=sum((m(2:3)-mu_axes)'*prec_axes*(m(2:3)-mu_axes))/length(mu_axes);
sec=min(10^15, -log(gmm.pdf(m(2:3)'))); % thresholding to avoid inf logarithm.
% set of heuristics, does not change much the results in most of the case, but avoid some catastrophic failures
if sec>1 && first<50
sec=sec*1000;
end
......@@ -182,7 +278,6 @@ if sec<-1 || first >50 %10
else %big reprojection error, low prior, I boost the prior
sec=sec*1000;
end
end
if v
first
......@@ -192,20 +287,11 @@ r=-1*( first + sec );
end
function r=log_hammer_pca(m,mu_data,prec_data,mu_axes,prec_axes,R,P,mu,v)
axes_norm=P*m(2:3)+mu;
first=sum((R*(m(1)*axes_norm)-mu_data)'*prec_data*(R*(m(1)*axes_norm)-mu_data));
sec=sum((m(2:3)-mu_axes)'*prec_axes*(m(2:3)-mu_axes));
if v
first
sec
end
r=-1*( first + sec );
end
function r=log_hammer(m,mu_data,prec_data,mu_axes,prec_axes,R,v)
first=sum((R*(m(1)*m(2:4))-mu_data)'*prec_data*(R*(m(1)*m(2:4))-mu_data));
sec=sum((m(2:4)-mu_axes)'*prec_axes*(m(2:4)-mu_axes));
function r=log_hammer_obs(m,mu_data,prec_data,gmm,R,P,mu,C,v)
axes=m(1)*sqrt(abs(P*m(2:3)+mu));axes=axes.^2;
obs_gen=C*R*axes;
first=((obs_gen-mu_data)'*prec_data*(obs_gen-mu_data));
sec=min(10^15, -log(gmm.pdf(m(2:3)'))); % thresholding to avoid inf logarithm.
if v
first
sec
......@@ -217,4 +303,4 @@ function c = cost(angles,G,C,l)
R=rot_euler2rot(angles)';
Rm=[ R(1,:).^2 ; R(1,:).*R(2,:) ; R(1,:).*R(3,:); R(2,:).^2; R(2,:).*R(3,:); R(3,:).^2];
c=norm(G*Rm*l-C);
end
\ No newline at end of file
end
function [C,Eh,scales,axes_red,Sigma_noise,out]=em_orient(prs,classes,measure,C,m_values,angles,Gs,inc,defaultNoise,Nmax)
if nargin < 10
Nmax = 1 % max number of iterations
end
[n_images3,n_obj]=size(measure);
Sigma_noise=eye(n_images3);
if defaultNoise~=0
fprintf('default noise modified %d\n',defaultNoise)
Sigma_noise=Sigma_noise*defaultNoise;
else
Sigma_noise=Sigma_noise;
end
Lold=-Inf;
passed=0;
Cold=C;
Sigma_noiseold=Sigma_noise;
Ehold=zeros(6,n_obj);
EhhOld=zeros(6,6,n_obj);
scales=zeros(n_obj,1);
axes=zeros(3,n_obj);
angles_pre=angles;
for n=1:Nmax
[Eh,Ehh,scales_post,axes_post,axes_red,angles_post,~,out]=h_exp_first(prs,classes,measure,C,Sigma_noise,axes,scales,angles_pre,Gs,inc);
if sum(isnan(Eh(:)))>0
disp('nan rate')
sum(isnan(Eh(:)))/numel(Eh)
end
Eh(isnan(Eh(:)))=Ehold(isnan(Eh(:)));
Eh(isinf(Eh(:)))=Ehold(isinf(Eh(:)));
Ehh(isnan(Ehh(:)))=EhhOld(isnan(Ehh(:)));
Ehh(isinf(Ehh(:)))=EhhOld(isinf(Ehh(:)));
%Mstep
diagnoise=zeros(n_images3,1);
%C=measure/Eh;
for i=1:n_images3
idx=(m_values(i,:)==1);%taking all observed points
if sum(idx~=0)~=0
%C(i,:)=(measure(i,idx)*Eh(:,idx)')/sum(Ehh(:,:,idx),3);
if defaultNoise==0
diagnoise(i)=measure(i,idx)*measure(i,idx)' - C(i,:)*Eh(:,idx)*measure(i,idx)';
diagnoise(i)=diagnoise(i)/sum(idx);
end
end
end
diagnoise(diagnoise<0.0001)=0.0001;%to avoid numerical precision error;
diagnoise(:)=mean(diagnoise);
% if there is default noise to use
if defaultNoise~=0
diagnoise(:)=defaultNoise;
end
Sigma_noise=diag(diagnoise);
% computing the log-likelihood
L=0;
for i=1:n_obj
ux=C*Eh(:,i);
Covx=Sigma_noise + C*eye(6)*C';
%L=L - 0.5*(n_images3*log(2*pi) + log(det(Covx(m_values(:,i)==1,m_values(:,i)==1))) + (measure(m_values(:,i)==1,i)-ux(m_values(:,i)==1))'*inv(Covx(m_values(:,i)==1,m_values(:,i)==1))*(measure(m_values(:,i)==1,i)-ux(m_values(:,i)==1)) );
L=L - 0.5*(n_images3*log(2*pi) + size(measure,1)*log(Covx(1,1)) + (measure(m_values(:,i)==1,i)-ux(m_values(:,i)==1))'*inv(Covx(m_values(:,i)==1,m_values(:,i)==1))*(measure(m_values(:,i)==1,i)-ux(m_values(:,i)==1)) );
end
%checking if the likelihood is still increasing.
Ls(n)=L;
if L>Lold
Lold=L;
Cold=C;
Ehold=Eh;
EhhOld=Ehh;
Sigma_noiseold=Sigma_noise;
scales=scales_post;
axes=axes_post;
angles_pre=angles_post;
elseif ~passed
passed=1;
else
break
end
end
C=Cold;
Sigma_noise=Sigma_noiseold;
Eh=Ehold;
if n==Nmax
fprintf('max number of iterations reached %d\n',n)
else
fprintf('quitting after %d/%d iterations\n',n,Nmax)
end
end
function [Eh,Ehh,scales,axes_post,axes_red,angles_post,chs,out]=h_exp_first(prs,classes,measure,C,Sigma_noise,axes_post,scales,angles_post,Gs,inc)
%computing posterior variance for each object.
[~,n_obj]=size(measure);
prec_data=inv(Sigma_noise);
size_h=size(C,2);
Eh=zeros(size_h,n_obj); %expected value of h under the distribution q(h)
Ehh=zeros(size_h,size_h,n_obj);%E[hh'] another sufficient statistics which will be needed for the M step
lastwarn('');
chs=zeros(n_obj,3);
axes_red=zeros(2,n_obj);
for i=1:n_obj
sfmo_axes=[ Gs{i}.e0 Gs{i}.e1 Gs{i}.e2]';
%for k=1:20
init_angle=angles_post(:,i);
R=rot_euler2rot(init_angle)';
Rm=[ R(1,:).^2 ; R(1,:).*R(2,:) ; R(1,:).*R(3,:); R(2,:).^2; R(2,:).*R(3,:); R(3,:).^2];
perm = perms(1:3);
gmm=prs.gmms(classes(i)).gmm;
sc=norm(sfmo_axes);
for cc=1:size(perm,1)
ch=perm(cc,:);
mu = prs.mus(ch,classes(i));
P = prs.ps(ch,classes(i)*2-1:classes(i)*2);
l=sfmo_axes;
l=l/sc;
h=P'*(l.^2-mu);
[~, msgid]=lastwarn;
logPfuns={@(m) log_hammer_obs2(m,measure(:,i),prec_data,gmm,Rm,P,mu,C,0)};
%init=[ linspace(h(1),0,8);linspace(h(2),0,8)]; minit=[ [0.1 1 10 100 0.1 1 10 100 ]*sc ; init];
minit=[ [0.1 1 10 100 0.1 1 10 100 ]*sc ;mvnrnd(repmat(h,1,8)',eye(2))' ];
%minit=[ [0.1 1 10 100 0.1 1 10 100 ]*sc ;zeros(2,8) ];
%minit=[ [0.1 1 10 100 0.1 1 10 100 0.1 1 10 100 0.1 1 10 100 ]*1000 ;10*mvnrnd(repmat(mu_axes,1,16)',prs.vars(:,classes(i)*2-1:classes(i)*2))' ];
%minit=[ [0.1 1 10 100 0.1 1 10 100 ]*1000 ;mvnrnd(repmat(mu_axes,1,8)',prs.vars(:,classes(i)*2-1:classes(i)*2))' ];
[modi,lgP]=gwmcmc(minit,logPfuns,1000,'ThinChain',1);
models(cc*3-2:cc*3,:)=modi(:,:);
logP(cc,:)=lgP(:,:);
[~,ii]=max(logP(cc,:));
Ehi=models(cc*3-2:cc*3,ii);
axes_norm=P*Ehi(2:3)+mu;
axes_norm(logical([ 0; axes_norm(2:3)<0 ]) ) = 0.1;
l=(Ehi(1)*sqrt(axes_norm)).^2;
out.Ehi(i,:,cc)=Rm*l;
out.costg(i,cc)=log_hammer_obs2(Ehi,measure(:,i),prec_data,gmm,Rm,P,mu,C,0);
out.coste(i,cc)=norm(C*Rm*l-measure(:,i));
end
[~,r]=max(max(logP,[],2));
[~,index]=max(logP(r,:));
Ehi=models(r*3-2:r*3,index);
chs(i,:)=perm(r,:);
ch=perm(r,:);
P=prs.ps(ch,classes(i)*2-1:classes(i)*2);
log_hammer_obs2(models(r*3-2:r*3,index),measure(:,i),prec_data,gmm,Rm,P,mu,C,1);
mu=prs.mus(ch,classes(i));
axes_red(:,i)=Ehi(2:3);
axes_norm=abs(P*Ehi(2:3)+mu);
l=(Ehi(1)*sqrt(axes_norm)).^2;
scales(i)=Ehi(1);
axes_post(:,i)=axes_norm;
obs=C*Rm*l;
%x = lsqnonlin(@(x) cost(x,C,obs,l),init_angle);
x=init_angle;
angles_post(:,i)=x;
R=rot_euler2rot(x)';
Rm=[ R(1,:).^2 ; R(1,:).*R(2,:) ; R(1,:).*R(3,:); R(2,:).^2; R(2,:).*R(3,:); R(3,:).^2];
Ehi=Rm*l;
covi=cov((Rm* ( repmat(models(r*3-2,:),3,1).*(P*models(r*3-1:r*3,:))))');
Ehhi=Ehi*Ehi' + covi;
%end
Eh(:,i)=Ehi;
Ehh(:,:,i)=Ehhi;
if 0
Rec_post = recombine_ellipsoids(Eh,inc.S1);
%transforming the quadrics into ellipsoids.
qvecs_sfmo=zeros(10,n_obj);
qvecs_psfmo=qvecs_sfmo;
for o=1:length(inc.GT)
qvecs_sfmo(:,o)=sym2vec(inc.Rec{o}.Q);
qvecs_psfmo(:,o)=sym2vec(Rec_post{o}.Q);
end
Rec_new=quadric2ellipsoide(qvecs_sfmo);
Rec_post_new=quadric2ellipsoide(qvecs_psfmo);
for o=1:length(inc.GT)
Rec_new{o}.Q=ellipsoide2quadric(Rec_new{o});
Rec_post_new{o}.Q=ellipsoide2quadric(Rec_post_new{o});
end
x3cGT = compCent(inc.GT);
[dummy,x3c_transf,transproc] = procrustes(real(x3cGT)',real(inc.S1)');
[ RecT ] = tranEllipsoid( Rec_new, transproc );
[mea,oRec,AP3D,table3D] = results3D(RecT,inc.GT,50000);
[ Rec_postT ] = tranEllipsoid( Rec_post_new, transproc );
[meap,oRec,AP3Dp,table3D] = results3D(Rec_postT,inc.GT,50000);
[dummy,x3c_transf,transproc] = procrustes(real(inc.S1)',real(x3cGT)');
[ GTT ] = tranEllipsoid( inc.GT, transproc );
if 0 %plotting
close all;
rec = plot3Dscene2(inc.GT, Rec_postT,x3cGT, n_obj, 0, [2 3 4], 9, 0, 0, [0 1 0],[0 0 1]);
[~] = plot3Dscene2(inc.GT, RecT, x3cGT, n_obj, 0, [], 9, 0, 0, [0 1 0],[1 0 0]);
rec = plot3Dscene2(inc.Rec, Rec_post,x3cGT, n_obj, 0, [], 9, 0, 0, [1 0 0],[0 0 1]);
%rec = plot3Dscene2(Rec_new, GTT, x3cGT, n_obj, 0, [], 9, 0, 0, [1 0 0],[0 0 1]);
hold on;
for f=1:length(inc.Imm)
A=inc.R1(2*f-1:2*f,:);
[V,l]=eig(A'*A);
[~,index]=min(diag(l));
line( [ 0 V(1,index) ], [ 0 V(2,index) ], [ 0 V(3,index) ]);
end
hold off;
Nf=length(inc.Imm);
%Crec = reprQ(inc.R1,inc.Rec,ones(Nf,n_obj),'ortho',1);
%Crec_post = reprQ(inc.R1,Rec_post,ones(Nf,n_obj),'ortho',1);
Crec = reprQ(inc.R1,Rec_new,ones(Nf,n_obj),'ortho',1);
%Crec_post = reprQ(inc.R1,GTT,ones(Nf,n_obj),'ortho',1);
Crec_post = reprQ(inc.R1,Rec_post_new,ones(Nf,n_obj),'ortho',1);
Crec = retrasl(Crec,inc.TT);
Crec_post = retrasl(Crec_post,inc.TT);
rowzero=[];
for f=1:max(1,round(length(inc.Imm)/5)):length(inc.Imm) %
if sum(rowzero==f)~=0
continue
end
figure;
imshow(inc.Imm{f}.I); hold on;
for o=1:(size(Crec_post,2)/3)
plotEllipse(inc.Cnotrasl(3*f-2:3*f,3*o-2:3*o)/(-inc.Cnotrasl(3*f,3*o)),[0 1 0],4); hold on;
plotEllipse(Crec(3*f-2:3*f,3*o-2:3*o)/(-Crec(3*f,3*o)),[1 0 0],4); hold on;
plotEllipse(Crec_post(3*f-2:3*f,3*o-2:3*o)/(-Crec_post(3*f,3*o)),[0 0 1],4); hold on;
end
end
end
[ AP3D AP3Dp]
end
a=1;
end
Sigma_noise(1);
b=2;
end
function r=log_hammer_obs2(m,mu_data,prec_data,gmm,R,P,mu,C,v)
axes=m(1)*sqrt(abs(P*m(2:3)+mu));axes=axes.^2;
obs_gen=C*R*axes;
first=((obs_gen-mu_data)'*prec_data*(obs_gen-mu_data))/length(mu_data);
sec=min(10^15, -log(gmm.pdf(m(2:3)'))); % thresholding to avoid inf logarithm.
% set of heuristics, does not change much the results in most of the case, but avoid some catastrophic failures
if sec>1 && first<50
sec=sec*1000;
end
if sec<-1 || first >50 %10
if sec<2 % big reprojection error, high prior, I boost the data cost
first=first*1000;
else %big reprojection error, low prior, I boost the prior
sec=sec*1000;
end
end
if v
first
sec
end
r=-1*( first + sec );
end
function r=log_hammer_obs(m,mu_data,prec_data,gmm,R,P,mu,C,v)
axes=m(1)*sqrt(abs(P*m(2:3)+mu));axes=axes.^2;
obs_gen=C*R*axes;
first=((obs_gen-mu_data)'*prec_data*(obs_gen-mu_data));
sec=min(10^15, -log(gmm.pdf(m(2:3)'))); % thresholding to avoid inf logarithm.
if v
first
sec
end
r=-1*( first + sec );
end
function c = cost(angles,G,C,l)
R=rot_euler2rot(angles)';
Rm=[ R(1,:).^2 ; R(1,:).*R(2,:) ; R(1,:).*R(3,:); R(2,:).^2; R(2,:).*R(3,:); R(3,:).^2];
c=norm(G*Rm*l-C);
end
clear all
close all
clc
method = 1;
AP3DRef = [];
diffAngRef = [];
dist3DRef = [];
addpath(genpath('../functions'));
% % ~~~~~~~~~~~~~~~~ Set all the variables ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Npflag = 0; Np = 0; % additional points are not present
gt_file = '/ssd_disk/datasets/scannet/material_for_psfmo_and_co/gt_pca.mat';
seqdir = '/ssd_disk/datasets/scannet/material_for_psfmo_and_co/scannet_seqs/';
scannet_dir = '/ssd_disk/datasets/scannet/data/';
prior_file = fullfile('..','stats.mat'); %prior statistics extracted from the shapenet dataset
gt=load(gt_file);
seqs=dir([ seqdir '/s*' ]); %