Commit 88e9db43 authored by Guillem Alenyà's avatar Guillem Alenyà
Browse files

used by marduengo to get the dataset

parents
This diff is collapsed.
This diff is collapsed.
#!/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()
#!/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
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
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment