%Einlesen der Kameraparameter
%camPar = dlmread('test.txt', '\t', 0, 1);
%Versuch etwas sinnvolles hinzubekommen
%camNumber = textscan('test.txt', '\bild', delimiter, '.jpg');
fileID = fopen('test.txt');
C = textscan(fileID,'%11s%f%2s  %f %f %f %f %f %f %f %f %f %f');
fclose(fileID);
Z = C{2};
camPar = [imag(C{2}),C{4},C{5},C{6},C{7},C{8},C{9},C{10},C{11},C{12}]

M1 = zeros(1092*1080, 4);
M2 = zeros(1, 4);

%Loop um für jedes Bild Daten zu bekommen
for i = 1:size(camPar, 1)

    %Intrinsische Kameraparameter
    K = [camPar(i, 2), camPar(i,10)/(camPar(i, 2))^2, 960
        0, camPar(i, 2), 540
        0, 0, 1];
    % Quaternion zu Rotationsmatrix
    Qrotation = [camPar(i, 3) camPar(i, 4) camPar(i, 5) camPar(i, 6)];

    w = Qrotation(1);
    x = Qrotation(2);
    y = Qrotation(3);
    z = Qrotation(4);

    Rxx = 1 - 2*(y^2 + z^2);
    Rxy = 2*(x*y - z*w);
    Rxz = 2*(x*z + y*w);

    Ryx = 2*(x*y + z*w);
    Ryy = 1 - 2*(x^2 + z^2);
    Ryz = 2*(y*z - x*w );

    Rzx = 2*(x*z - y*w );
    Rzy = 2*(y*z + x*w );
    Rzz = 1 - 2 *(x^2 + y^2);

    R = [
        Rxx,    Rxy,    Rxz;
        Ryx,    Ryy,    Ryz;
        Rzx,    Rzy,    Rzz];

    %Translation
    T = [camPar(i, 7); camPar(i, 8); camPar(i, 9)];
    %Berechnung der Projektionsmatrix

    RT = [R,-R*T];
    RT0 = [RT; 0 0 0 1];
    K0 = [K, zeros(3, 1)];
    KRT = [K0*RT0; 0,0,0,1];
    KRTI = inv(KRT);

    %Laden der Depthsmap
    newDepth = load(['Bilder\depth' int2str(camPar(i, 1)) '.txt'],'-ascii');
    newDepth = newDepth/168;
    %% room for your implementation of a 3D-reconstruction
    for j = 1:4:1080
        for k = 1:4:1920
            l = sub2ind([1080, 1920], j, k);
            %Pointcloud erzeugen
           if(newDepth(j, k) <= 750/168 && newDepth(j,k) >= 400/168)
                M1(l,:) = (KRT\([k*newDepth(j, k); j*newDepth(j, k); newDepth(j, k); 1]))';
           end
        end
    end
    %Anhängen an große Pointcloud
    M2 = [M2;M1];
    i
end

'Ich bin fast fertig'

M2 = M2(:,1:end-1);
M2( all(M2 ==0, 2), :) = [];

%showPointCloud(M2)
% Speichern der Pointcloud
header = 'ply\n';
header = [header, 'format ascii 1.0\n'];
header = [header, 'element vertex ', num2str(size(M2, 1)), '\n'];
header = [header, 'property float x\n'];
header = [header, 'property float y\n'];
header = [header, 'property float z\n'];
header = [header, 'element face 0\n'];
header = [header, 'property list uchar int vertex_indices\n'];
header = [header, 'end_header\n'];

fid = fopen('test.ply', 'w');
fprintf(fid, header);
dlmwrite('test.ply', M2, '-append', 'delimiter', '\t', 'precision', 3);
fclose(fid);