data_modified

Local2GlobalMap.m

@@ -0,0 +1,13 @@
+function [ fullCloud ] = Local2GlobalMap( localCloud , MotionGlobal ,s ,mergeGrid )
+%LOCAL2GLOBALMAP 此处显示有关此函数的摘要
+%   此处显示详细说明
+fullCloud=localCloud{1};
+for tar=2:length(localCloud)
+    transMotion=MotionGlobal{tar};
+    transMotion(1:3,4)=MotionGlobal{tar}(1:3,4)./s;
+    TranCData= transMotion*[localCloud{tar}.Location';ones(1,localCloud{tar}.Count)];
+    tcloud=pointCloud( TranCData(1:3,:)');
+    fullCloud=pcmerge(fullCloud,tcloud,mergeGrid);
+end
+end
+

demo.m

@@ -3,7 +3,7 @@
 addpath('./flann/');
 addpath('./estimateRigidTransform');
 
-gridStep = 0.01;
+gridStep = 0.02;
 datapath = './data';
 
 srcFileName = 'data/bun000.ply';

eigMatch.m

@@ -17,7 +17,7 @@
 distThr = 0.2/4*length(radii); 
 thetaThr = 10; 
 threshold = gridStep*gridStep;
-  for i = 1:ceil(0.3*N)
+  for i = 1:ceil(0.5*N)
     n= id(i);
 %   for n = 1:N
     seed = srcSeed(:,seedIdx(n));
@@ -80,7 +80,7 @@
         [dist,ind] = sort(dist);        
         Err(n) = sum(sum((tarEst(:,index(ind(1:ovNum)))-tarSeed(:,ind(1:ovNum))).^2));
     end
-    if (size(matches,1)> 0.12*size(tarDesp,2))
+    if (size(matches,1)> 0.15*size(tarDesp,2))
         break;
     end
  end

extractEig.m

@@ -28,7 +28,7 @@
     srcIdx = rangesearch(srcData',srcSeed',radii(k));
     [sk,nk] = cellfun(@(x,y)svdCov(x,y,srcData,srcSeed),srcIdx,idx,'uni',false);
     s = [s cell2mat(sk)];
-    n = [n cell2mat(nk)];
+    n = [n cell2mat(nk)]; 
 end
 s = s';
 ds = diff(s);

helpful_script.m

@@ -1,32 +1,26 @@
 
-%% 真值结果读取
+%% csv全部点云根据真值配准
+
    for i=1:size(icpList,1)
         GrtM{i}=[icpList(i,1:4);icpList(i,5:8);icpList(i,9:12);icpList(i,13:16)];
    end
-   GrtM=GrtM';
+%    GrtM=GrtM';
 
 %% 真值误差
 %scannum=length(clouds);
 % for i=1:scannum
-%     Data_{i}=clouds{i}.Location;
-%     
+%     Data_{i}=clouds{i}.Location;    
 % end
-% 
 % RotErr=0;
 % TranErr=0;
 % scannum=length(GrtR);
-% 
 % for i=1:scannum
 %     RotErr=norm((p(i).M(1:3,1:3)-GrtR{i}),'fro')+RotErr;
 %     TranErr=norm((p(i).M(1:3,4)-GrtT{i}),2)+TranErr;
-%     
-%    
 % end
 % RotErr=RotErr/scannum
 % TranErr=TranErr/scannum
 %  RotLogErr=log(RotErr)
-% 
-% 
 
 
 % clc;
@@ -62,7 +56,7 @@
 %     curMotion=p(i).M;
 %     curMotion(1:3,4)=curMotion(1:3,4)./s;
     pcshow(pcdenoise(clouds{i}));
-
+end
 
 
 

localization.m

@@ -1,70 +1,80 @@
 
 clc;clear;close all;
-gridStep=0.04;
+gridStep=0.025;
 load  Clear_OutSide;
 load outside_regis_motion_result;
 load outside_GRT;
 addpath('./flann');
 addpath('./estimateRigidTransform/');
-
-fullPointCloud=clouds{1};
+mergeGrid=0.002;
+% fullPointCloud=clouds{1};
 overlap=0.4;
 res=10;
-
-for i=2:N
-    if(i~=22)
-        transMatrix=p(i).M';
-        transMatrix(4,1:3)=transMatrix(4,1:3)./s;
-        transFormCloud{i}=pctransform(clouds{i},affine3d(transMatrix));
-        fullPointCloud=pcmerge(fullPointCloud,transFormCloud{i},0.002);
+fullPointCloud=Local2GlobalMap(clouds,GrtM,s,mergeGrid); 
+[tarDesp,tarSeed,tarNorm] = extractEig(fullPointCloud,gridStep);
+for tar= 12
+
+
+%     for i=2:N
+%         if(i~=tar)
+%             transMatrix=p(i).M';
+%             transMatrix(4,1:3)=transMatrix(4,1:3)./s;
+%             transFormCloud{i}=pctransform(clouds{i},affine3d(transMatrix));
+%             fullPointCloud=pcmerge(fullPointCloud,transFormCloud{i},0.002);
+%         end
+%     end
+
+    tic
+    [srcDesp,srcSeed,srcNorm] = extractEig(clouds{tar},gridStep);
+    T = eigMatch(tarDesp,srcDesp,tarSeed,srcSeed,tarNorm,srcNorm,overlap,gridStep);
+    T = inv(T);
+    R0= T(1:3,1:3);
+    t0= T(1:3,4)*s;
+
+%     
+    Model= fullPointCloud.Location(1:res:end,:)'*s;
+    Data= clouds{tar}.Location(1:res:end,:)'*s;
+%     TData = transform_to_global(Data, R0, t0);
+    [MSE,R,t,TData,PCorr, Dthr] = TrICP(Model, Data, R0, t0, 100, overlap);
+    Motion=Rt2M(R,t);
+    MotionbackUp=Motion;
+    toc
+    disp(['cloud ' num2str(tar) ' tested']);
+    RotErr=norm((MotionbackUp(1:3,1:3)-GrtM{tar}(1:3,1:3)),'fro')
+
+    TranErr=norm((MotionbackUp(1:3,4)-GrtM{tar}(1:3,4)),2)
+    if(RotErr>0.1||TranErr>0.1)
+        disp(['cannot relocate cloud '  num2str(tar)]);
     end
 end
-
-tic
-[tarDesp,tarSeed,tarNorm] = extractEig(fullPointCloud,gridStep);
-[srcDesp,srcSeed,srcNorm] = extractEig(clouds{22},gridStep);
-T = eigMatch(tarDesp,srcDesp,tarSeed,srcSeed,tarNorm,srcNorm,overlap,gridStep);
-T = inv(T); 
-R0= T(1:3,1:3);
-t0= T(1:3,4)*s;
-
-
-Model= fullPointCloud.Location(1:res:end,:)'*s;
-Data= clouds{22}.Location(1:res:end,:)'*s;
-% TData = transform_to_global(Data, R0, t0);
-[MSE,R,t,TData,PCorr, Dthr] = TrICP(Model, Data, R0, t0, 100, overlap); 
-Motion=Rt2M(R,t);
-MotionbackUp=Motion;
-toc
-
-RotErr=norm((MotionbackUp(1:3,1:3)-GrtM{22}(1:3,1:3)),'fro')
-
-TranErr=norm((MotionbackUp(1:3,4)./s-GrtM{22}(1:3,4)),2)
-
-
+%
 Motion=MotionbackUp;
-pnum=clouds{22}.Count;
-toshow=pointCloud(clouds{22}.Location,'Color',uint8([ zeros(pnum,1) ones(pnum,1) zeros(pnum,1)]));
+pnum=clouds{tar}.Count;
+toshow=pointCloud(clouds{tar}.Location,'Color',uint8([ zeros(pnum,1) ones(pnum,1) zeros(pnum,1)]));
 Motion=Motion';
 Motion(4,1:3)=Motion(4,1:3)./s;
 pcshow( pctransform(toshow, affine3d(Motion)));
 hold on
 
 pcshow(fullPointCloud);
+xlabel('x');
+ylabel('y');
+zlabel('z');
+axis([-1 1 -1 1 -0.2 0.6 ]);
 %չʾ·¾¶Í¼
-figure;
+% figure;
 for i=1:N
-    if(i~=22)
-        cameraPosition(i,:)=p(i).M(1:3,4)';
+    if(i~=tar)
+        cameraPosition(i,:)=GrtM{i}(1:3,4)'./s;
     else
         cameraPosition(i,:)=cameraPosition(i-1,:);
     end
 
 end
-plot(cameraPosition(:,1),cameraPosition(:,2),'-*')
+plot3(cameraPosition(:,1),cameraPosition(:,2),cameraPosition(:,3),'r-*');
 hold on;
-plot(MotionbackUp(1,4),MotionbackUp(2,4),'bo')
-xlabel('x');
-ylabel('y');
-
-% axis([-0.5 0.5 -0.5 0.5 ]);
+plot3(MotionbackUp(1,4)/s,MotionbackUp(2,4)/s,MotionbackUp(3,4)/s,'bo');
+% xlabel('x');
+% ylabel('y');
+%
+% % axis([-0.5 0.5 -0.5 0.5 ]);

localization_apart.m

@@ -1,33 +1,27 @@
 clc;clear;close all;
 gridStep=0.04;
-overlap=0.2;
+overlap=0.3;
 res=1;
 addpath('./flann');
 addpath('./estimateRigidTransform/');
-filepath='./data/global_frame_ap/';
-prefix='PointCloud';
+filepath='./data/local_frame_ap/';
+prefix='Hokuyo_';
 readNum=45;
 N=readNum;
-mergeGridStep=0.001;
+% mergeGridStep=0.0000005;
 s=15;
 relocoNum=10;%重定位几号
-fullPointCloud = readCloudAsOne(filepath,prefix,readNum,mergeGridStep,s,relocoNum);
-load apartment_noupper;
-load apartmentGrT;
+% load apartment_Grt;
+% fullPointCloud = readCloudAsOne(filepath,prefix,readNum,mergeGridStep,s,relocoNum,MotionGlobalGT);
 
-% fullPointCloud=clouds{1};
-% for i=2:N
-%     if(i~=relocoNum)
-%         transMatrix=p(i).M';
-%         transMatrix(4,1:3)=transMatrix(4,1:3)./s;
-%         transFormCloud{i}=pctransform(clouds{i},affine3d(transMatrix));
-%         fullPointCloud=pcmerge(fullPointCloud,transFormCloud{i},0.002);
-%     end
-% end
+load apartment_CloudsClean;
+load apartment_FullClean_big;
+load apartment_GrT;
 
-tic
+tic;
 [tarDesp,tarSeed,tarNorm] = extractEig(fullPointCloud,gridStep);
 [srcDesp,srcSeed,srcNorm] = extractEig(clouds{relocoNum},gridStep);
+toc;disp('des generated');
 T = eigMatch(tarDesp,srcDesp,tarSeed,srcSeed,tarNorm,srcNorm,overlap,gridStep);
 T = inv(T); 
 R0= T(1:3,1:3);
@@ -41,9 +35,9 @@
 % Motion=Rt2M(R,t);
 % MotionbackUp=Motion;
 toc
-% % 
-% RotErr=norm((MotionbackUp(1:3,1:3)-MotionGrt{relocoNum}(1:3,1:3)),'fro')
-% TranErr=norm((MotionbackUp(1:3,4)-MotionGrt{relocoNum}(1:3,4)),2)
+
+RotErr=norm((MotionbackUp(1:3,1:3)-MotionGrt{relocoNum}(1:3,1:3)),'fro')
+TranErr=norm((MotionbackUp(1:3,4)-MotionGrt{relocoNum}(1:3,4))./s,2)
 
 % Motion=MotionbackUp;
 pnum=clouds{relocoNum}.Count;

readCloudAsOne.m

@@ -1,14 +1,15 @@
-function fullPointCloud = readCloudAsOne(filepath,prefix,readNum,mergeGridStep,s,relocoNUm)
+function fullPointCloud = readCloudAsOne(filepath,prefix,readNum,mergeGridStep,s,relocoNUm,GrtM)
 %READCLOUDASONE 此处显示有关此函数的摘要
 %   此处显示详细说明
 readNum=readNum-1;
 clouds=readCloudCsv(filepath,prefix,readNum,0,s);
-fullPointCloud=clouds{1};
-for i=2:length(clouds)
-%     if i~=relocoNUm
-        fullPointCloud=pcmerge(fullPointCloud,clouds{i},mergeGridStep);
-        disp([ 'cloud ' num2str(i) ' merged!'] );
-%     end
-end
+fullPointCloud=Local2GlobalMap(clouds,GrtM,s,mergeGridStep);
+
+% for i=2:length(clouds)
+% %     if i~=relocoNUm
+%         fullPointCloud=pcmerge(fullPointCloud,clouds{i},mergeGridStep);
+%         disp([ 'cloud ' num2str(i) ' merged!'] );
+% %     end
+% end
 end
 

readCloudCsv.m

@@ -4,54 +4,32 @@
 if(readnum>length(dir(filepath))-2) 
     error('OutOfRange!');
 end
-for i=0:readnum
-%     filename=[filepath 'PointCloud' num2str(i) '.csv'];
+ for i=0:readnum
+
     filename=[filepath filePrefix num2str(i) '.csv'];
     % Load data
     cloud = importdata(filename);
-
-
-    %% 仅用于临时转换,应删除
-%     load apartmentGrT;
-%     localcloud= pctransform( pointCloud(cloud.data(:,2:4)),affine3d(MotionGrt{i+1}'));
-
+    sky=0.14;
     %% 去除自身点与地面
-%     pointupper= localcloud.Location(:,3)>1.4*s ;   %天花板
-%     pointGroud= localcloud.Location(:,3)<zlimit*s ;% 会拍到机器人自身的部分形成大干扰
-%     pointrobat= localcloud.Location(:,3)>zlimit*s & localcloud.Location(:,3)<0.023*s & localcloud.Location(:,1)<0.06*s & localcloud.Location(:,2)<0.025*s & localcloud.Location(:,2)>-0.025*s;
-%     pointBad=pointGroud | pointrobat | pointupper;
-%     pointSelect=~pointBad;
-%     pointLocation=cloud.data(pointSelect,2:4)./s;
-%     
-    pointupper= cloud.data(:,4)>0.14*s ;   %天花板
-%     pointGroud= cloud.data(:,4)<zlimit*s ;% 地面
-%     pointrobat= cloud.data(:,4)>zlimit*s & localcloud.data(:,4)<0.023*s & localcloud.data(:,2)<0.06*s & localcloud.data(:,3)<0.025*s & localcloud.data(:,3)>-0.025*s;
-    pointBad=  pointupper;  % pointrobat |pointGroud |
+    if i==41
+        sky=0.12;
+    end
+    pointupper= cloud.data(:,4)>sky*s ;   %天花板
+    pointGroud= cloud.data(:,4)<zlimit*s ;% 地面
+    pointrobat= cloud.data(:,4)>zlimit*s & cloud.data(:,4)<0.05*s & cloud.data(:,2)<0.06*s & cloud.data(:,3)<0.025*s & cloud.data(:,3)>-0.025*s;
+    pointBad=  pointrobat |pointGroud |pointupper;  % 
     pointSelect=~pointBad;           
 
-
-    % Extract coordinates
-%     x = cloud.data(:, strcmp('x', cloud.colheaders));
-%     y = cloud.data(:, strcmp('y', cloud.colheaders));
-%     z = cloud.data(:, strcmp('z', cloud.colheaders));
-    %更换为快捷但不健壮的表达 Extract coordinates
-
-    pointGroud= cloud.data(:,4)<zlimit*s ;% 会拍到机器人自身的部分形成大干扰
-    pointrobat= cloud.data(:,4)>zlimit*s & cloud.data(:,4)<0.023*s & cloud.data(:,2)<0.06*s & cloud.data(:,3)<0.025*s & cloud.data(:,3)>-0.025*s;
-    pointBad=pointGroud | pointrobat;
-    pointSelect=~pointBad;
-%     
-%     pointSelect= cloud.data(:,4)>-11 ;
-%     pointSelect=xor(pointSelectLoud,pointrobat);
-%     pointLocation=cloud.data(:,2:4)./s;
-
     pointLocation=cloud.data(pointSelect,2:4)./s;
 
     %% 不修剪
 %     pointLocation=cloud.data(:,2:4)./s;
 
     %%
     pointClouds{(i+1)}= pointCloud(pointLocation);
+
+
+
 end
 end