Commit b38131ce authored by Paul Gay's avatar Paul Gay
Browse files

creation of the repo

parents
function [ frVw, bbx ] = findFramesKinect( base_dir, startF, endF )
load([base_dir './kinect_office_release110929/images/0/image' num2str(startF) '.mat']);
n_o = size(bboxGT,1);
n_f = endF-startF+1;
frVw = zeros(n_f,n_o);
bbx = NaN(n_f,4*n_o);
for f=1:n_f
load([base_dir './kinect_office_release110929/images/0/image' num2str(startF+f-1) '.mat']);
for o=1:n_o
bbxTmp = bboxGT(o,1:4);
bbxCnd = bbxTmp(1)>0 & bbxTmp(2)>0 & bbxTmp(3)<640 & bbxTmp(4)<480;
if bbxCnd == 1
bbx(f,4*o-3:4*o) = bbxTmp;
frVw(f,o) = 1;
end
end
end
end
function [A , c] = MinVolEllipse(P, tolerance)
% [A , c] = MinVolEllipse(P, tolerance)
% Finds the minimum volume enclsing ellipsoid (MVEE) of a set of data
% points stored in matrix P. The following optimization problem is solved:
%
% minimize log(det(A))
% subject to (P_i - c)' * A * (P_i - c) <= 1
%
% in variables A and c, where P_i is the i-th column of the matrix P.
% The solver is based on Khachiyan Algorithm, and the final solution
% is different from the optimal value by the pre-spesified amount of 'tolerance'.
%
% inputs:
%---------
% P : (d x N) dimnesional matrix containing N points in R^d.
% tolerance : error in the solution with respect to the optimal value.
%
% outputs:
%---------
% A : (d x d) matrix of the ellipse equation in the 'center form':
% (x-c)' * A * (x-c) = 1
% c : 'd' dimensional vector as the center of the ellipse.
%
% example:
% --------
% P = rand(5,100);
% [A, c] = MinVolEllipse(P, .01)
%
% To reduce the computation time, work with the boundary points only:
%
% K = convhulln(P');
% K = unique(K(:));
% Q = P(:,K);
% [A, c] = MinVolEllipse(Q, .01)
%
%
% Nima Moshtagh (nima@seas.upenn.edu)
% University of Pennsylvania
%
% December 2005
% UPDATE: Jan 2009
%%%%%%%%%%%%%%%%%%%%% Solving the Dual problem%%%%%%%%%%%%%%%%%%%%%%%%%%%5
% ---------------------------------
% data points
% -----------------------------------
[d N] = size(P);
Q = zeros(d+1,N);
Q(1:d,:) = P(1:d,1:N);
Q(d+1,:) = ones(1,N);
% initializations
% -----------------------------------
count = 1;
err = 1;
u = (1/N) * ones(N,1); % 1st iteration
% Khachiyan Algorithm
% -----------------------------------
while err > tolerance,
X = Q * diag(u) * Q'; % X = \sum_i ( u_i * q_i * q_i') is a (d+1)x(d+1) matrix
M = diag(Q' * inv(X) * Q); % M the diagonal vector of an NxN matrix
[maximum j] = max(M);
step_size = (maximum - d -1)/((d+1)*(maximum-1));
new_u = (1 - step_size)*u ;
new_u(j) = new_u(j) + step_size;
count = count + 1;
err = norm(new_u - u);
u = new_u;
end
%%%%%%%%%%%%%%%%%%% Computing the Ellipse parameters%%%%%%%%%%%%%%%%%%%%%%
% Finds the ellipse equation in the 'center form':
% (x-c)' * A * (x-c) = 1
% It computes a dxd matrix 'A' and a d dimensional vector 'c' as the center
% of the ellipse.
U = diag(u);
% the A matrix for the ellipse
% --------------------------------------------
A = (1/d) * inv(P * U * P' - (P * u)*(P*u)' );
% center of the ellipse
% --------------------------------------------
c = P * u;
function [ P_vec ] = Pvectz( P )
%PVECTZ Summary of this function goes here
% Detailed explanation goes here
n_f = size(P,1)/4;
for f=1:n_f
Pf = P(4*f-3:4*f,:)';
P_vec(f,1:12) = Pf(:)';
end
end
function [ Cnoisy ] = addAngNoise( C, maxAng, randV )
%ADDANGNOISE Summary of this function goes here
% Detailed explanation goes here
n_o = size(C,2)/3;
n_f = size(C,1)/3;
Cnoisy = zeros(size(C,1),size(C,2));
for o=1:n_o
for f=1:n_f
% [~, ~, c, ~] = explodeCparam(C(f*3-2:f*3,o*3-2:o*3));%RUBINO
[~, c, ~] = explodeparam( C(f*3-2:f*3,o*3-2:o*3) );
Ctemp = C(f*3-2:f*3,o*3-2:o*3); %CROCCO
% T = [1 0 -c(1); 0 1 -c(2); 0 0 1];
% T1 = [1 0 c(1); 0 1 c(2); 0 0 1]
% Ctemp1 = T*Ctemp*T';
%c = -Ctemp(1:2,3); %CROCCO
% % ~ Cancellare
% figure; plotEllipse(C(3*f-2:3*f,3*o-2:3*o),[1 0 0]); pause;
% % ~ Cancellare
alphaGrad = 2*maxAng*(1-2*randV(f,o));
alpha = alphaGrad/180*pi;
Rtmp = [cos(alpha),-sin(alpha);sin(alpha),cos(alpha)];
% RR = [[Rtmp, [0;0]]; [0 0 1]];
% Cn = T1*RR*Ctemp1*(RR')*T1';
% Cnoisy(f*3-2:f*3,o*3-2:o*3) = invAdj([Rtmp,-Rtmp*c + c;0 0 1]*adjA(C(f*3-2:f*3,o*3-2:o*3),3)*[Rtmp,-Rtmp*c + c;0 0 1]');%RUBINO
% Cnoisy(f*3-2:f*3,o*3-2:o*3) = [Rtmp,-Rtmp*c + c;0 0 1]*Ctemp*[Rtmp,-Rtmp*c + c;0 0 1]';%CROCCO
Cnoisy(f*3-2:f*3,o*3-2:o*3) = [Rtmp,-Rtmp*c + c;0 0 1]*Ctemp*[Rtmp,-Rtmp*c + c;0 0 1]';%CROCCO
% Cnoisy(f*3-2:f*3,o*3-2:o*3) = Cnoisy(f*3-2:f*3,o*3-2:o*3);%/Cnoisy(f*3,o*3);
% Cnoisy(f*3-2:f*3,o*3-2:o*3) = Cnoisy(f*3-2:f*3,o*3-2:o*3)/sign(Cnoisy(f*3-2,o*3-2));
% % ~ Cancellare
% plotEllipse(Cnoisy(3*f-2:3*f,3*o-2:3*o),[0 1 0]);pause;
% % ~ Cancellare
end
end
end
function [ Cnoisy ] = addAxNoise( C, maxVarAx, randV )
%ADDANGNOISE Summary of this function goes here
% Detailed explanation goes here
n_o = size(C,2)/3;
n_f = size(C,1)/3;
Cnoisy = zeros(size(C,1),size(C,2));
for o=1:n_o
for f=1:n_f
% [ax1, ax2, c, R] = explodeCparam(C(f*3-2:f*3,o*3-2:o*3));%RUBINO
[ax, c, R] = explodeparam( C(f*3-2:f*3,o*3-2:o*3));
varAx = maxVarAx*(1-2*randV(f,o));
% Cnoisy(f*3-2:f*3,o*3-2:o*3) = invAdj([R,c;0 0 1]*adjA(diag([(ax1*(1+varAx))^(-2),(ax2*(1+varAx))^(-2),-1]),3)*[R,c;0 0 1]');%RUBINO
Cnoisy(f*3-2:f*3,o*3-2:o*3) =[R,c;0 0 1]*diag([(ax(1)*(1+varAx))^2,(ax(2)*(1+varAx))^2,-1])*[R,c;0 0 1]';%RUBINO
% Cnoisy(f*3-2:f*3,o*3-2:o*3) = Cnoisy(f*3-2:f*3,o*3-2:o*3)/Cnoisy(f*3,o*3); %RUBINO
% Ellipse_plotHm2D(C(f*3-2:f*3,o*3-2:o*3),[1 0 0]);
% Ellipse_plotHm2D(Cnoisy(f*3-2:f*3,o*3-2:o*3),[0 0 1]); axis equal;
% pause;
end
end
end
function [ Cnoisy ] = addCentNoise( C, maxCvar, randV, randVangC )
%add noise to the ellipsoid centres. It guess it takes the average size of the axes, then draw a uniform value between half of this average and 1.5 this average. Then multiply again by the maxCvar parameter.
%
n_o = size(C,2)/3;
n_f = size(C,1)/3;
Cnoisy = zeros(size(C,1),size(C,2));
for o=1:n_o
for f=1:n_f
% [ax1, ax2, ~, ~] = explodeCparam(C(f*3-2:f*3,o*3-2:o*3)); % RUBINO
[ax,~,~] = explodeparam( C(f*3-2:f*3,o*3-2:o*3) );
% Ctemp = C(f*3-2:f*3,o*3-2:o*3); %CROCCO
% c = -Ctemp(1:2,3); %CROCCO
% T = [1 0 -c(1); 0 1 -c(2); 0 0 1];
% Ctemp = T*C(f*3-2:f*3,o*3-2:o*3)*T';
% [V,D] = eig(Ctemp(1:2,1:2));
% ax1 = sqrt(D(1,1));
% ax2 = sqrt(D(2,2));
alpha = 2*pi*randV(f, o);
% avgAx = mean([ax1 ax2])*[cos(alpha),sin(alpha)]';
avgAx = mean(ax)*[cos(alpha),sin(alpha)]';
centNoise = avgAx*(1-2*randVangC(f, o))*maxCvar;
% adjA(C(f*3-2:f*3,o*3-2:o*3),3)
% C(f*3-2:f*3,o*3-2:o*3)
% Cnoisy(f*3-2:f*3,o*3-2:o*3) = invAdj([eye(2),centNoise;0 0 1]*adjA(C(f*3-2:f*3,o*3-2:o*3),3)*[eye(2),centNoise;0 0 1]');%RUBINO
Cnoisy(f*3-2:f*3,o*3-2:o*3) = [eye(2),centNoise;0 0 1]*C(f*3-2:f*3,o*3-2:o*3)*[eye(2),centNoise;0 0 1]';
% adjA(Cnoisy(f*3-2:f*3,o*3-2:o*3),3)
% Cnoisy(f*3-2:f*3,o*3-2:o*3)
% Cnoisy(f*3-2:f*3,o*3-2:o*3) = Cnoisy(f*3-2:f*3,o*3-2:o*3)/Cnoisy(f*3,o*3);%RUBINO
% figure;
% Ellipse_plotHm2D(C(f*3-2:f*3,o*3-2:o*3),[1 0 0]);
% Ellipse_plotHm2D(Cnoisy(f*3-2:f*3,o*3-2:o*3),[0 0 1]); axis equal;
% pause;
end
end
end
function bbx=addNoiseAxes(bbx,n)
n=min(0.5,n);
for i=1:size(bbx,1)
for j=1:size(bbx,2)/4
x1=bbx(i,j*4-3);y1=bbx(i,j*4-2);x2=bbx(i,j*4-1);y2=bbx(i,j*4);
sx=(x2-x1)*rand()*n;sy=(y2-y1)*rand()*n;
bbx(i,j*4-3:j*4)=[x1-sx/2 y1-sy/2 x2+sx/2 y2+sy/2 ];
end
end
\ No newline at end of file
function [ Aadj ] = adjA( A)
%ELLIPSEADJ computes the adjoint of a matrix A with size sz.
%
%SYNTAX
%
% [ Aadj ] = adjA( A, sz )
%
%INPUTS
%
% A [Nsz x Nsz DOUBLE] : This is the collection of input matrices on which
% we want to compute the adjoint. We can use it for
% one or many matrices combined together.
% sz [1 x 1 DOUBLE] : This is the size of the matrix A.
%
%OUTPUT
%
% A [Nsz x Nsz DOUBLE] : This matrix is composed by the collection of
% adjoint matrices computed from A.
%
% See also INVADJ
sz=size(A,1);
n_o = size(A,2)/sz;
n_f = size(A,1)/sz;
Aadj = zeros(size(A));
for o=1:n_o
for f=1:n_f
a = A((f-1)*sz+1:f*sz,(o-1)*sz+1:o*sz);
Aadj((f-1)*sz+1:f*sz,(o-1)*sz+1:o*sz) = (a^(-1))*det(a);
end
end
end
\ No newline at end of file
function [ C ] = bbx2ell( BB , sz)
%FROMBB2ELL Summary of this function goes here
% Detailed explanation goes here
% plane q is tangent to the conic when:
% 0 = q S^-1 q^t
% = q M^-1 M S^-1 M^t M^t^-1 q^t
% = (q M^-1) (M S^-1 M^t) (q M^-1)^t
%
% transformed plane (q M^-1) should preserve incidence
% -> dual conic transformed by matrix M is: (M S^-1 M^t)
% BB = [xmin ymin xmax ymax]
if nargin<2
sz='out';
end
if strcmp(sz,'out')
a = sqrt(2)*abs(BB(3)-BB(1))/2;
b = sqrt(2)*abs(BB(4)-BB(2))/2;
else
a = abs(BB(3)-BB(1))/2;
b = abs(BB(4)-BB(2))/2;
end
x0 = [(BB(1)+BB(3))/2 (BB(2)+BB(4))/2];
Ccn = [diag([1/a^2 1/b^2]),[0 0]';[0 0 -1]];
P = [eye(2),x0';[0 0 1]];
Cinv = P*(Ccn^(-1)*det(Ccn))*P';
%C = invAdj(Cinv);% Rubino
C = Cinv; % CROCCO
end
\ No newline at end of file
% Script to run the Structure from Motion with Objects algorithm
%
% If you use this code please cite the following paper:
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
% "Structure from Motion with Objects",
% Marco Crocco, Cosimo Rubino, Alessio del Bue
% IEEE Computer Society Conference on Computer Vision
% and Pattern Recognition (CVPR), 2016.
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%
% Created by Marco Crocco, Cosimo Rubino and Alessio Del Bue
% Last version: 15 april 2016
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
clear all
close all
%clc
method = 1;
AP3DRef = [];
diffAngRef = [];
dist3DRef = [];
addpath(genpath('../../../fa_objects/functions'));
addpath('../../../fa_objects/grinsted-gwmcmc-2c86b0e/');
% % ~~~~~~~~~~~~~~~~ Set all the variables ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
seq = 9; % Selected sequence
Npflag = 0; Np = 0; % additional points are not present
base_dir = 'C:\Users\pgay\fotos\bottles_seq\cropped';
trackfile = 'C:\Users\pgay\fotos\bottles_seq\results\results_frame_number.txt';
%base_dir = 'C:\Users\pgay\fotos\chair_bonzai_more\cropped';
%trackfile = 'C:\Users\pgay\fotos\chair_bonzai_more\results\postprocess\results_frame_number.txt';
[Imm, GT, frVw0, bbx ] = new_seq_AUTOdetection(base_dir,trackfile);
% Put the indexes of all the objects viewed the scene
gtViewedObj = listObjects(GT);
frVw = frVw0;
tic
Rec = cell(size(GT));
% Compute the outline ellipses for the reconstruction
C = fromBBx2ell(bbx,frVw);
[Nf1,No1] = size(frVw);
if 0
f=29;
imshow(Imm{f}.I); hold on;
for i=1:(size(bbx,2)/4)
if sum(isnan(bbx(f,(i*4-3):(i*4))))==0
%plotBbx(bbx(f,(i*4-3):(i*4)),[1 1 0],2,i);
plotEllipse(C(3*f-2:3*f,3*i-2:3*i)/(-C(3*f,3*i)),[1 0 0],4); hold on;
end
end
end
colzero=[5 8 9 10];
col_ok=1:No1;
col_ok(colzero)=[];
a=bbx(:,1:4:end);
rowzero = find(isnan(sum(a(:,col_ok),2)) | sum(a(:,col_ok)==0,2)>0 );
row_ok=1:Nf1;
row_ok(rowzero)=[];
toremove=[1:12 16:56 ]; % seq 4
rowzero=sort([rowzero' row_ok(toremove)]);
row_ok(toremove)=[];
Cfull = []; points1 = []; points2 = [];
f1 = 1;
for f = 1 : Nf1
if min(abs(f-rowzero)) > 0
Crow = []; Crowp = [];
for o = 1 : No1
if min(abs(o-colzero)) > 0
Crow = [Crow, C(3*(f-1)+1:3*(f-1)+3,3*(o-1)+1:3*(o-1)+3)];
end
end
for p = 1 : Np
Ctempp = [ 1, 0, -points(2*(f1-1)+1,p); 0, 1, -points(2*(f1-1)+2,p); -points(2*(f1-1)+1,p), -points(2*(f1-1)+2,p), -1 ];
Crowp = [Crowp Ctempp];
end
f1 = f1+1;
Cfull = [Cfull; [Crow, Crowp]];
end
end
C = Cfull;
[Nf,No] = size(C);
Nf = Nf/3;
No = size(Crow,2)/3;
Np = (size(C,2)- size(Crow,2))/3;
Cnotrasl = C;
%A key step to reduce the complexity of the problem consists in enforcing a
%translation of each image frame according to the average of the ellipses coordinate centers
[C, TT ] = frametrasl(C);
% ~~~~~~~~~~~~~~~~~~~~~~~~ Quadrics Fitting ~~~~~~~~~~~~~~~~~~~~~~~~~~
%initialise Cadjv_mat but let G_est and Q_est as [].
[ G_est, Q_est, Cadjv_mat ] = generateQuadrics_and_CamPose(C,method);
% Structure from Motion given ellipse centers (corresponding to
% projections of ellipsoid centers): solving for translation
[Pred, Tred] = sfm_from_centers(Cadjv_mat);
Rtilde = Pred;
Stilde = Tred;
% Enforces orthogonality constraints on Projection matrices: R1 are
% projection mat , S1 are ellipsoid cent
[R1, S1] = fact_metric_constraint(Rtilde,Stilde);
% Build a rank 6 reduced matrix, eliminating rows and columns related
% to translation
Gred = rebuild_Gred(R1,method);
% Remove center from ellipses (it is equivalent to center ellipsoids
% in the orthographic case),
[Ccenter,centers,~] = center_ellipses(Cadjv_mat,1);
% Estimate centered ellipsoid given the projection matrices : solving for
% shape and size
Quadrics_centered = Gred\Ccenter;
% Add previously estimated centers to the ellipsoid
Rec = recombine_ellipsoids(Quadrics_centered,S1);
%starting the code for psfmo
prs=get_shapenet_prs('C:\Users\pgay\fotos\shapenetCore\stats3.mat');
classes=[2 2 2 4 4 2];
% build the axes for each ellipsoid prior.
[Gs]=quadric2ellipsoide([ Quadrics_centered(1:3,:) ; zeros(1,size(Quadrics_centered,2)); Quadrics_centered(4:5,:); zeros(1,size(Quadrics_centered,2)); Quadrics_centered(6,:) ; zeros(1,size(Quadrics_centered,2)) ; ones(1,size(Quadrics_centered,2)) ] );
angles=zeros(3,length(classes));
for i=1:length(classes)
R=[Gs{i}.u1 Gs{i}.u2 Gs{i}.u3];
if det(R) < 0
R=[ Gs{i}.u1 Gs{i}.u2 -Gs{i}.u3];
end
angles(:,i) = rot_rot2euler(R);
end
method=1;
Gred = rebuild_Gred(R1,method);
[C,Eh,Sigma]=em_1_img(prs,classes,Ccenter,Gred,ones(size(Ccenter,1),size(Ccenter,2)),angles,Gs,GT);
Rec_post = recombine_ellipsoids(Eh,S1);
% ~~~~~~~~~~~~~~~~~~~~~~~~~~ Plotting ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%[~] = plot3Dscene( Priors_Ell, S1, No1, Np, colzero, seq, Npflag);
[~] = plot3Dscene( Rec_post, S1, No1, Np, colzero, seq, Npflag);
rec = plot3Dscene( Rec, S1, No1, Np, colzero, seq, Npflag, R1);
[~] = plot3Dscene2( Rec_post, Rec, S1, No1, Np, colzero, seq, Npflag);
% camera3D(c,orientation,size)
%
% camera3D generate a 3D wireframe of a stylized camera, to represent
% the direction of the point of view. ON the camera there is a frame wich
% shows the 3 axis
%
%
% x axes = freen
% y axes = blue
% z axes = red
%
% _________________________________________________________________________
%
% -I/O
%
% Given:
%
% c [3 x 1 double] Camera centre. It may be evaluated from the
% [Rt] matrix, by calculating -R'*t
%
% rotmat [3 x 3 double] Camera orientation. IT may be generate from
% the [Rt] by transposing (R')
%
% size [1 x 1 double] A constant wich represent the size of
% the camera.
% _________________________________________________________________________
%
% the call:
%
% camera3D(c,orientation,size)
%
% returns:
%
% Camera wireframe oriented in the world reference frame
%
% -Examples
%
% camera3D([0 0 0]',eye(3,3),10)
% ________________________________________________________________________
% Created by Cosimo Rubino
% Last Modified: 03/10/2013
function camera3D(c,orient,size,color)
if nargin<4
color=[0 0 1];
end
% Points of the cube
C_points1 =[1 1 1 1 0 0 0 0; ...
1 1 0 0 1 1 0 0; ...
1 0 1 0 1 0 1 0 ];
% Points of the trapezioid
C_points2 =[.75 1 .75;
.25 1 .75;
.25 1 .25;
.75 1 .25;
1 2 1;
1 2 0;
0 2 1;
0 2 0;
]';
% Join of the cube and the trapezoid
Cam.points = [C_points1 C_points2];
Cam.points = Cam.points + repmat([-.5 -1 -.5]',1,length(Cam.points));
Cam.points = [1 0 0; 0 0 -1; 0 1 0]*Cam.points;
% Apply the rotations and the traslation to the camera wireframe
Cam.points = size.*orient*Cam.points + repmat(c',1,length(Cam.points));
% Point to generate the wireframe lines
lines = [1 2 4 3 7 8 6 5 1 2 6 5 7 8 4 3 1 9 10 11 12 14 16 14 13 9 13 15 10 15 16 11 16 11 12 9 1 ;
2 4 3 7 8 6 5 1 2 6 5 7 8 4 3 1 9 10 11 12 14 16 14 13 9 13 15 10 15 16 11 16 11 12 9 1 2];
% Plot of the camera
plot3([Cam.points(1,lines(1,:)) Cam.points(1,lines(2,:))],...
[Cam.points(2,lines(1,:)) Cam.points(2,lines(2,:))],...
[Cam.points(3,lines(1,:)) Cam.points(3,lines(2,:))],'-k','linewidth',1.5,'color',color);
hold on;
frame_repr(c',size.*orient);
axis equal;
\ No newline at end of file
function [ A, C ] = cart2homo( MX )
%CART2HOMO gets the equation of the conic or quadric and its center
%
%SYNTAX
%
% [ A, C ] = cart2homo( MX )
%
%INPUT
%
% MX [N X N DOUBLE]: The input conic or the quadric
%
%OUTPUT
%
% A [N-1 x N-1 DOUBLE]: The matrix which represents the shape
%
% C [N-1 x 1 DOUBLE]: The vector which represents the spatial location of
% the shape
%
if ~isempty(MX)
n = length(MX);
A = MX(1:n-1,1:n-1);
C = -(A^(-1))*MX(1:n-1,n);
else
A = [];
C = [];
end
end
function [Ccenter,centers,Cpluscenter] =center_ellipses(Cmat,method,weigth_ell)
[Nf,No] = size(Cmat);
Nf = Nf/6;
if method == 2 || method == 3
for f = 1 : Nf
for o = 1 : No
Cvet = squeeze(Cmat(6*(f-1)+1:6*(f-1)+6,o));
Cvet1 = Cvet;
T = [1 0 Cvet(3); 0 1 Cvet(5); 0 0 1 ];
C = vec2sym(Cvet');
C = T*C*