EuRoc的groundTruth选取camera部分位姿

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%读取.csv文件中的groudTruth,与camera时间戳对齐,保存位姿
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 close all;clc;clear;
 file_name1 = '/data.csv';
 file_name2 = '/cam0/data.csv';
 savepath =  '/out.txt';
 data1 = csvread(file_name1, 1, 0);%读取第1行以下，第0列以右区域的数据；  csvread仅能读取纯数字
 data2 = csvread(file_name2, 1, 0);
 [m1,n1] = size(data1);
 [m2,n2] = size(data2);
 num = 0;
 for i = 1:m1
     for j =1:m2
         x1 = data1(i,1);
         x11 =uint64(x1);
         x2 = data2(j,1);
         x22= uint64(x2);
         y1 = num2str(x11);
         y11 = y1(1:16);
         y2 = num2str(x22);
         y22 = y2(1:16);
     if(y11 == y22)
         num = num+1;
         data4(num,:) = data1(i,2:8);
     end
     end
 end
 [m3,n3] = size(data4);
 fid = fopen(savepath,'wt');
 for j=1:m3    %输出数字
     fprintf(fid,'%f,%f,%f,%f,%f,%f,%f\n',data4(j,1),data4(j,2),data4(j,3),data4(j,4),data4(j,5),data4(j,6),data4(j,7));
 end
 fclose(fid);

 

输入文件1:data.csv格式：

#timestamp, p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z [], v_RS_R_x [m s^-1], v_RS_R_y [m s^-1], v_RS_R_z [m s^-1], b_w_RS_S_x [rad s^-1], b_w_RS_S_y [rad s^-1], b_w_RS_S_z [rad s^-1], b_a_RS_S_x [m s^-2], b_a_RS_S_y [m s^-2], b_a_RS_S_z [m s^-2]
1403636580838555648,4.688319,-1.786938,0.783338,0.534108,-0.153029,-0.827383,-0.082152,-0.027876,0.033207,0.800006,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
1403636580843555328,4.688177,-1.786770,0.787350,0.534640,-0.152990,-0.826976,-0.082863,-0.029272,0.033992,0.804771,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
1403636580848555520,4.688028,-1.786598,0.791382,0.535178,-0.152945,-0.826562,-0.083605,-0.030043,0.034999,0.808240,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
1403636580853555456,4.687878,-1.786421,0.795429,0.535715,-0.152884,-0.826146,-0.084391,-0.030230,0.035853,0.810462,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
1403636580858555648,4.687727,-1.786240,0.799484,0.536244,-0.152821,-0.825731,-0.085213,-0.029905,0.036316,0.811406,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
1403636580863555328,4.687579,-1.786059,0.803540,0.536768,-0.152768,-0.825314,-0.086049,-0.029255,0.036089,0.811225,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
1403636580868555520,4.687435,-1.785881,0.807594,0.537289,-0.152725,-0.824896,-0.086890,-0.028469,0.035167,0.810357,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593

 

输入文件2:/cam0/data.csv格式：

#timestamp [ns],filename
1403636579763555584,1403636579763555584.png
1403636579813555456,1403636579813555456.png
1403636579863555584,1403636579863555584.png
1403636579913555456,1403636579913555456.png
1403636579963555584,1403636579963555584.png
1403636580013555456,1403636580013555456.png
1403636580063555584,1403636580063555584.png

 

输出文件out.txt格式：

4.687579,-1.786059,0.803540,0.536768,-0.152768,-0.825314,-0.086049
4.686208,-1.784735,0.843777,0.541956,-0.152228,-0.821204,-0.093543
4.684712,-1.784429,0.883348,0.547733,-0.151265,-0.817072,-0.097593
4.683646,-1.784502,0.920697,0.553298,-0.149681,-0.813376,-0.099524
4.682709,-1.784758,0.955649,0.558956,-0.149126,-0.809612,-0.099450
4.681407,-1.783794,0.988490,0.564328,-0.149483,-0.805690,-0.100275
4.680835,-1.783124,1.018420,0.570891,-0.151428,-0.800702,-0.100201