used by marduengo to get the dataset

extract_time.py

+#!/usr/bin/env python
+"""
+Extracts:
+ - File name (it uses the timestamp as file name)
+ - Sequence
+ - Timestamp
+ - Date
+ - Hour
+ - Frame id
+ from the pointcloud messages published in the topic '/kinect2/qhd/points' and
+ stored in the rosbag and writes them to terminal.
+ The output will be redirected to a .txt and .csv file
+"""
+import rosbag
+import datetime
+
+# open rosbag
+bag = rosbag.Bag('bag_name.bag')
+
+# print output file header
+print "file_name sequence timestamp date hour frame_id"
+
+# for each message, read the data to extract
+for topic, msg, t in bag.read_messages(topics=['/kinect2/qhd/points']):
+    date = datetime.datetime.fromtimestamp(msg.header.stamp.to_sec()).strftime("%Y-%m-%d %H:%M:%S.%f")
+    
+    # create string to name the file from the timestamp
+    n=0
+    a=''
+    for i in str(msg.header.stamp):
+	a=a+i
+	if n==9:
+		a=a+'.'
+	n=n+1
+	
+	# write data line
+    print a, msg.header.seq, msg.header.stamp, date, msg.header.frame_id
+    
+# close rosbag    
+bag.close()

pcd2png.sh

+#!/bin/bash
+
+# Create folders to store depth and color images
+mkdir depth_images
+mkdir color_images
+
+cd pointclouds
+
+# for each pointcloud in the folder pointclouds ...
+for f in *; do 
+# ... create the depth image and store it in its folder
+pcl_pcd2png --field z "$f" "../depth_images/${f%.*}_depth.png" 
+# ... create the color image and store it in its folder
+pcl_pcd2png "$f" "../color_images/${f%.*}_color.png"
+done

sincroniza.m

+function sincroniza(track_file,kinect_file,varargin)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% The script "sincroniza.m" selects the time frame when both 
+% Optitrack y la Kinect have recorded images. It associates to each image or
+% frame recorded by the Kinect camera a frame recorded by the Optitrack, using
+% the condition that the diference in the recording time between both must
+% be minimum.
+%
+% Next, using the data in the Optitrack file (rotation quaternion and
+% position of the Kinect reference system) it transforms the coordinates of
+% the reflectors to the Kinect reference system.
+%
+% Each pair of associated frames is writen in the same line in the output
+% files. 
+%
+%%%%%%%%%%%%%%%%%%%%%
+% Input arguments
+%%%%%%%%%%%%%%%%%%%%%
+%
+% track_file: name, with folder path, of the Optitrack file in .csv format
+% kinect_file: name, with folder path, of the Kinect file in .csv format
+% output_folder: name, with folder path, of the output folder 
+%
+%%%%%%%%%%%%%%%%%%%%%
+% Output files
+%%%%%%%%%%%%%%%%%%%%%
+%
+% There are four output files:
+%
+% - A .csv file that contains the coordinates of the reflectors in the Kinect
+%   reference system, constructed from track_file but that retains only the
+%   synchronized frames (rows) and where the first column is the name of
+%   the corresponding Kinect frame. Its name is the same as the original
+%   track_file but with "_out_kinect_ref" appended.
+%
+% - A .mat file that contains the same data as the previous file (but with
+%   only one header line) but stored as Matlab dataset. The name is the same as
+%   the previous one but with .mat extension.
+%
+% - A .csv file that contains the coordinates of the reflectors in the
+%   Optitrack reference system, constructed from track_file but that retains
+%   only the synchronized frames (rows) and where the first column is the name of
+%   the corresponding Kinect frame. Its name is the same as the original
+%   track_file but with "_out_optitrack_ref" appended.
+%
+% - A .mat file that contains the same data as the previous file (but with
+%   only one header line) but stored as Matlab dataset. The name is the same as
+%   the previous one but with .mat extension.
+% 
+% This script stores this four files in the folder
+% "correspondence_kinect-optitrack":
+% a) The defect setting stores it in the Escritorio (ONLY FOR UBUNTU IN
+%    SPANISH)
+% B) If there is a third argument, that is the folder which contains
+    "correspondence_kinect-optitrack"
+%%%%%%%%%%%%%%%%%%%%%
+% Example
+%%%%%%%%%%%%%%%%%%%%%
+% sincroniza('/home/username/Desktop/e1/Take 2016-07-26 12.07.50 AM.csv',...
+%            '/home/username/Desktop/e2/time.csv',...
+%            '/home/username/Desktop/e')
+%
+% The output files will be in the folder
+% /home/username/Desktop/e/correspondence_kinect-optitrack
+%
+% Next, only for Ubuntu in spanish:
+%
+% sincroniza('/home/user/Escritorio/e1/Take 2016-07-26 12.07.50 AM.csv',...
+%            '/home/user/Escritorio/e2/time.csv')
+%
+% The output files will be in the folder
+% /home/username/Escritorio/correspondence_kinect-optitrack
+%%
+tic  % To measure running time
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% PART 1: Load data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% We establish the output folder
+if nargin < 2
+    error('Not enough arguments.');
+elseif nargin == 2
+    % output_folder='~/Desktop';% Mac
+    output_folder='~/Escritorio';% Ubuntu Spanish
+    disp('The output folder is "correspondence_kinect-optitrack" and it is in the Escritorio')
+    disp('We are assuming that the output folder is')
+    disp(output_folder)
+    disp('If that is not the case, give a folder with path as third argument')
+    disp('to store the output files')
+elseif nargin ==3
+    output_folder=varargin{1};
+    disp('The output folder is "correspondence_kinect-optitrack"')
+end
+mkdir(output_folder,'correspondence_kinect-optitrack')
+
+% Load Optitrack .csv file
+table_op = dataset('File',track_file,'ReadVarNames',true,...
+    'ReadObsNames',false,'Delimiter',',','HeaderLines',6);
+% Extract time for each Optitrack frame
+time=table_op.Time;
+
+% Load Kinect .csv file
+table_ki = dataset('File',kinect_file,'ReadVarNames',true,...
+    'ReadObsNames',false,'Delimiter',' ');
+% Extract hour for each Kinect frame
+hour=table_ki.hour;
+
+% Extract kinect file name as string (first column kinec_file)
+[mc,~]=size(table_ki);
+file_name_kinect=cell(mc,1);
+fid=fopen(kinect_file);
+line = fgets(fid);
+for i=2:mc+1
+   line = fgets(fid);
+   name_li=[];
+   k=1;
+   while ~strcmp(line(k),' ')
+       name_li=[name_li,line(k)];
+       k=k+1;
+   end
+   file_name_kinect{i-1}=name_li;
+end
+
+% Remove path from track_file name and store in track_file2
+pos=length(track_file);
+while ~strcmp(track_file(pos),'/')
+    pos=pos-1;
+end
+track_file2=track_file(pos+1:length(track_file));
+
+% output file names
+kinect_out = [output_folder,'/correspondence_kinect-optitrack/',track_file2(1:end-4),'_out_kinect_ref.csv'];
+kinect_out2 = [output_folder,'/correspondence_kinect-optitrack/',track_file2(1:end-4),'_out_kinect_ref.mat'];
+optitrack_out = [output_folder,'/correspondence_kinect-optitrack/',track_file2(1:end-4),'_out_optitrack_ref.csv'];
+optitrack_out2 = [output_folder,'/correspondence_kinect-optitrack/',track_file2(1:end-4),'_out_optitrack_ref.mat'];
+%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% PART 2: Transform time scale in Kinect to time scale of OptiTrack
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Establish Time Zero
+
+% Read the first line in OptiTrack file, that contains Time Zero in
+% OptiTrack
+fid = fopen(track_file);
+tline = fgets(fid);
+% and store the next 5 lines, to recover later the .csv file header
+tline1 = fgets(fid);
+tline2 = fgets(fid);
+tline3 = fgets(fid);
+tline4 = fgets(fid);
+tline5 = fgets(fid);
+
+% Look for the positions of ',' in the string of the first line in .csv
+coma=[];
+for i=1:length(tline)
+    if tline(i)==','
+        coma=[coma,i];
+    end
+end
+
+% Time Zero is between coma 9 and coma 10
+% Extract the string in between comas that contains date and time Zero
+t0 = tline(coma(9)+1:coma(10)-1);
+fprintf('\n\nTime Zero in OptiTrack file:\n\n %s\n\n',t0)
+fprintf('It records data for %.3f seconds\n',time(end))
+
+
+% Extract hour and trasform to number
+hour0 = str2double(t0(12:13));
+
+% Extract minute and trasform to number
+minute0 = str2double(t0(15:16));
+
+% Extract AM or PM
+AMorPM = t0(end-1:end);
+
+% Extract seconds and trasform to number
+l = length(t0);
+t1=t0;
+t1([1:17,l-2:l])=[];
+second0 = str2double(t1);
+
+% Establish Time Zero in seconds
+if strcmp(AMorPM,'AM') % if AM
+    time0 = 60*60*hour0 + 60*minute0 + second0;
+else % if PM add 12 hours
+    time0 = 60*60*(hour0+12) + 60*minute0 + second0;
+end
+
+% Transform "hour" (time) in Kinect to a number synchronized with time 
+% in OptiTrack
+% Read hour, minute and second in Kinect
+m=length(hour); % number of characters in time string
+
+% Preallocate matrices for each quantity (Kinect hour, minute and second)
+hour_k = zeros(m,1);
+minute_k = zeros(m,1);
+second_k = zeros(m,1);
+
+% Extract strings corresponding to Kinect hour, minute and second and 
+% transform to number
+for i=1:m
+    string = hour{i};
+    hour_k(i) = str2double(string(1:2));
+    minute_k(i) = str2double(string(4:5));
+    string(1:6)=[]; % remove hour and minute and we are left with second
+    second_k(i) = str2double(string);
+end
+
+
+% Time in the same scale for Kinect and OptiTrack
+time_kinect = 60*60*hour_k + 60*minute_k + second_k - time0;
+time_track = time;
+
+fprintf('\n\n\nInitial time for Kinect:\n\n %s\n\n',hour{1})
+fprintf('It records data for %.3f seconds\n\n\n\n',time_kinect(end)-time_kinect(1))
+%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% PART 3: Pair OptiTrack frames with Kinect frames (minimum time difference)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% vectors to store frames (rows) we keep in Kinect and OptiTrack
+kinect_row = [];
+track_row = zeros(m,1);
+
+% For each row in the Kinect file
+for i=1:m
+    % only for the Kinect frames within the OptiTrack time range
+    if  min(time_track) <= time_kinect(i) && time_kinect(i) <= max(time_track)
+        
+        kinect_row = [kinect_row;i]; % keep only the synchronized rows
+        
+        % compare with the times for the OptiTrack frames
+        for j=1:length(time_track)
+            % If the time of the Kinect frame is between two consecutive
+            % times of the OptiTrack frames
+            if time_track(j) <= time_kinect(i)  && time_kinect(i) <= time_track(j+1)
+                % ... the Kinect frame is paired with the closest one
+                if abs(time_track(j)-time_kinect(i)) < abs(time_track(j+1)-time_kinect(i))
+                    track_row(i) = j;
+                else
+                    track_row(i) = j+1;
+                end
+            end
+        end
+        
+        
+    end
+end
+
+% keep only the OptiTrack frames (rows) that were paired (synchronized)
+% (non synchronized rows did not change value in the preallocated matrix and are zero)
+remove=find(track_row==0);
+track_row(remove)=[];
+
+% If there are not kept rows it means that there is not intersection
+% between the recording times of the Kinect and the OptiTrack. We finish.
+if size(kinect_row)==0
+    fprintf('The recording times of Kinect and OptiTrack do not intersect!\n')
+    fprintf('Look at the recording times above!\n')
+    return
+end
+
+% Check the synchronization (paired times must be very close)
+check=[(0:length(track_row)-1)' time_kinect(kinect_row), time_track(track_row),...
+    time_kinect(kinect_row)-time_track(track_row)];
+% maximum difference time between two synchronized frames (rows) 
+max_diff=max(abs(check(:,4)));
+%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% PART 4: Remove non synchronized frames (rows)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Modify OptiTrack table
+
+% Keep only synchronized frames
+table_op_new = table_op(track_row,:);
+
+% Modify Kinect table
+
+% Keep only synchronized frames
+table_ki_new = table_ki(kinect_row,:);
+
+% Keep only names of synchronized frames
+file_name_kinect=file_name_kinect(kinect_row,:);
+
+% Use the first column in the tables for the kinect frame name
+% Change variable name and its content, substituting the number (the name
+% had been stored as a number and now is incorrect due to rounding error)
+% by the string read directly from the Kinect file
+table_op_new.Properties.VarNames{1} = 'kinect_file_name';
+table_op_new.kinect_file_name=file_name_kinect;
+
+table_ki_new.Properties.VarNames{1} = 'kinect_file_name';
+table_ki_new.kinect_file_name=file_name_kinect;
+%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% PART 5: Transform the coordinates from the OptiTrack reference system to 
+%         the Kinect reference system
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+[mt,nt]=size(table_op_new);
+table_op_new_ki_ref=table_op_new;
+
+for i=1:mt
+    % Rotation quaternion
+    q = [table_op_new.X(i) table_op_new.Y(i) table_op_new.Z(i) table_op_new.W(i)];
+    % Kinect centroid
+    t = [0 table_op_new.X_1(i) table_op_new.Y_1(i) table_op_new.Z_1(i)];
+    
+    % Inverse quaternion
+    qr = qInv(q)';
+    % Conjugate quaternion
+    qrc= qConj(qr)';
+    
+    % Transform markers coordinates
+    for k=11:4:23
+        r12= double(table_op_new(i,k));
+        r13= double(table_op_new(i,k+1));
+        r14= double(table_op_new(i,k+2));
+        r1=[0,r12,r13,r14];
+        
+        % r0 = qr*r1*qrc
+        r1 = r1-t;
+        r0=qMul(r1,qrc)';
+        r0=qMul(qr,r0)';
+        
+        % store new values (kinect ref) in place of the old ones (optitrack ref)
+        table_op_new_ki_ref(i,k)=dataset(r0(2));
+        table_op_new_ki_ref(i,k+1)=dataset(r0(3));
+        table_op_new_ki_ref(i,k+2)=dataset(r0(4));
+        
+    end
+    % Transform reflector coordinates
+    for k=27:3:(nt-2)
+        r12= double(table_op_new(i,k));
+        r13= double(table_op_new(i,k+1));
+        r14= double(table_op_new(i,k+2));
+        r1=[0,r12,r13,r14];
+        
+        % r0 = qr*r1*qrc
+        r1 = r1-t;
+        r0=qMul(r1,qrc)';
+        r0=qMul(qr,r0)';
+        
+        % store new values (kinect ref) in place of the old ones (optitrack ref)
+        table_op_new_ki_ref(i,k)=dataset(r0(2));
+        table_op_new_ki_ref(i,k+1)=dataset(r0(3));
+        table_op_new_ki_ref(i,k+2)=dataset(r0(4));
+        
+    end
+end
+%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% PART 6: Write output files
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Write output file in Kinect reference system as .csv
+export(table_op_new_ki_ref,'File',kinect_out,'Delimiter',',','WriteVarNames',true)
+
+% Add header, line by line
+prepend2file( tline5, kinect_out, false );
+prepend2file( tline4, kinect_out, false );
+prepend2file( tline3, kinect_out, false );
+prepend2file( tline2, kinect_out, false );
+prepend2file( tline1, kinect_out, false );
+prepend2file( tline, kinect_out, false );
+
+% Write output file in Kinect reference system as .mat (MATLAB)
+kinect=table_op_new_ki_ref;
+save(kinect_out2,'kinect')
+
+% Write output file in OptiTrack reference system as .csv
+export(table_op_new,'File',optitrack_out,'Delimiter',',','WriteVarNames',true)
+
+% Add header, line by line
+prepend2file( tline5, optitrack_out, false );
+prepend2file( tline4, optitrack_out, false );
+prepend2file( tline3, optitrack_out, false );
+prepend2file( tline2, optitrack_out, false );
+prepend2file( tline1, optitrack_out, false );
+prepend2file( tline, optitrack_out, false );
+
+% Write output file in OptiTrack reference system as .mat (MATLAB)
+optitrack=table_op_new;
+save(optitrack_out2,'optitrack')
+%
+toc   % To measure running time
+end
+%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% Functions to prepend lines
+% MathWorks Support Team on 20 Jan 2010
+%
+function prepend2file( string, filename, newline )
+
+% newline:  is an optional boolean, that if true will append a \n to the end
+% of the string that is sent in such that the original text starts on the
+% next line rather than the end of the line that the string is on
+% string:  a single line string
+% filename:  the file you want to prepend to
+tempFile = tempname;
+fw = fopen( tempFile, 'wt' );
+if nargin < 3
+    newline = false;
+end
+if newline
+    fwrite( fw, sprintf('%s\n', string ) );
+else
+    fwrite( fw, string );
+end
+fclose( fw );
+appendFiles( filename, tempFile );
+copyfile( tempFile, filename );
+delete(tempFile);
+% append readFile to writtenFile
+end
+
+
+function status = appendFiles( readFile, writtenFile )
+fr = fopen( readFile, 'rt' );
+fw = fopen( writtenFile, 'at' );
+while feof( fr ) == 0
+    tline = fgetl( fr );
+    fwrite( fw, sprintf('%s\n',tline ) );
+end
+fclose(fr);
+fclose(fw);
+end
+
+%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% Quaternions
+% by Przemyslaw Baranski
+%  
+% 05 Mar 2012
+% An easy-to-use set of functions for quaternion calculus. 
+
+
+function d = qLength( Q )
+% qLength: quaternion length (norm)
+% d = qLength( Q )
+% IN:
+%     Q - input quaternion
+%
+% OUT:
+%     d - quaternion's norm (length)
+%
+% VERSION: 03.03.2012
+
+d = sqrt( sum( Q .* Q ));
+end
+
+
+function Q = qMul( Q1, Q2 )
+% qMul: quaternion multiplication
+% IN:
+%     Q1 - first quaternion
+%     Q2 - second quaternion
+%
+% OUT:
+%     Q - output quaternion, Q = Q1*Q2
+%
+% REMARKS:
+%     1) Quaternion multiplication is not commutative, i.e. Q1*Q2 != Q2*Q1
+%     2) Quaternion multiplication is associative, i.e. Q1*Q2*Q3 = Q1*(Q2*Q3)=(Q1*Q2)*Q3
+%
+% VERSION: 03.03.2012
+
+s1 = Q1(1);
+s2 = Q2(1);
+v1 = Q1(2:4);
+v2 = Q2(2:4);
+
+s =s1*s2 - dot( v1,v2);
+v = s1*v2 + s2*v1 + cross( v1, v2 );
+v = reshape( v, 3, 1 );
+Q = [s;v];
+end
+
+
+function Q = qConj( Q1 )
+% qConj: quaternion conjugation
+% Q = qConj( Q1 )
+% IN:
+%     Q1 - input quaternion
+%
+% OUT:
+%     Q - output quaternion
+%
+% VERSION: 03.03.2012
+
+Q1 = reshape( Q1, 4, 1 );
+Q = [Q1(1);-Q1(2:4)];
+end
+
+
+function Q = qInv( Q1 )
+% qInv: quaternion reciprocal (inverse)
+% Q = qInv( Q1 )
+% IN:
+%     Q1 - input quaternion
+%
+% OUT:
+%     Q - reciprocal of Q1, i.e. Q1*Q = 1
+%
+% VERSION: 03.03.2012
+
+Q = qConj( Q1 ) ./ qLength( Q1 )^2;
+end