diff --git a/CI_initialize.m b/CI_initialize.m deleted file mode 100644 index c218f9e..0000000 --- a/CI_initialize.m +++ /dev/null @@ -1,34 +0,0 @@ - -theta=-pi/2:0.36*pi/180:pi/2; -ris = 0.15; -r0=4/ris; - -y=r0*sin(theta); -x=r0*cos(theta); - -x_g=floor(x); -y_g=floor(y); - -max_x = max(x_g); -max_y = max(y_g); - -n_mis = 1000;%length(laserScan_xy); - -occ_mat = zeros(max_x,2*max_y); -lid_mat = zeros(max_x,2*max_y); - -room_length = 49; -room_width = 49; - -lgth = floor(room_length/ris); -wdt = floor(room_width/ris); - -Global_map = zeros(lgth,wdt,3); -Cost_map = zeros(lgth-1,wdt-1,3); - -center_x = floor(lgth/2); -center_y = floor(wdt/2); - -laserScan_xy = cell.empty; -j=1; -kk=1; \ No newline at end of file diff --git a/Main.m b/Main.m index 76f7153..eb5340b 100644 --- a/Main.m +++ b/Main.m @@ -14,15 +14,22 @@ % map = Map("Load", #landamark); % map = Map("Load", #landamark, "auto"); % map = Map("Load", #landamark, "manual"); -map = Map('new', 100, 100); +map = Map('new', 60, 60); figure('units','normalized','outerposition',[0 0 1 1]); axis equal axis([0, 100, 0, 100]) map.plotMap(); print('map100x100','-depsc','-r0') -%% -parfor (k = 1:7, 4) - for n = 1:5 - multirobot(n, k * 150, map, 1) +%% Set-up paramaters simulation +% define number of robots to use +numrobot = 4; +% define base time +basetime = 200; +% increment of time +step = 9; + +parfor (k = 1:step, 4) + for n = 1:numrobot + multirobot(n, k * basetime, map, 1) end end diff --git a/Particle_Filter/AngleWrapping.m b/Particle_Filter/AngleWrapping.m deleted file mode 100644 index 7fed732..0000000 --- a/Particle_Filter/AngleWrapping.m +++ /dev/null @@ -1,8 +0,0 @@ -function angle = AngleWrapping(angle) - -if(angle>pi) - angle=angle-2*pi; -elseif(angle<-pi) - angle = angle+2*pi; -end; -end \ No newline at end of file diff --git a/Particle_Filter/DoObservationModel.m b/Particle_Filter/DoObservationModel.m deleted file mode 100644 index a8e5a90..0000000 --- a/Particle_Filter/DoObservationModel.m +++ /dev/null @@ -1,10 +0,0 @@ -function [z] = DoObservationModel(xVeh, iFeature,Map) - -Delta = Map(1:2,iFeature)-xVeh(1:2); - -z = [norm(Delta); - atan2(Delta(2),Delta(1))-xVeh(3)]; - -z(2) = AngleWrapping(z(2)); - -end \ No newline at end of file diff --git a/Particle_Filter/GetObservation.m b/Particle_Filter/GetObservation.m deleted file mode 100644 index 359264a..0000000 --- a/Particle_Filter/GetObservation.m +++ /dev/null @@ -1,21 +0,0 @@ -function [z,iFeature] = GetObservation(k) - -global Map; -global xTrue; -global RTrue; -global nSteps; - - -%fake sensor failure here -if (rand(1) > 0.3) -z = []; -iFeature = -1; - -else -if (Map()) -iFeature = ceil(size(Map,2)*rand(1)); -z = DoObservationModel(xTrue, iFeature,Map)+sqrt(RTrue)*randn(2,1); -z(2) = AngleWrapping(z(2)); -end; - -end \ No newline at end of file diff --git a/Particle_Filter/GetOdometry.m b/Particle_Filter/GetOdometry.m deleted file mode 100644 index 87cef58..0000000 --- a/Particle_Filter/GetOdometry.m +++ /dev/null @@ -1,16 +0,0 @@ -function [xnow] = GetOdometry(k,Robot,it) - -global LastOdom; %internal to robot low-level controller -global UTrue; - -if(isempty(LastOdom)) -global xTrue; -LastOdom = xTrue; -end; - -u = GetRobotControl(k,Robot,it); -xnow =tcomp(LastOdom,u); -uNoise = sqrt(UTrue)*randn(3,1); -xnow = tcomp(xnow,uNoise); -LastOdom = xnow; -end \ No newline at end of file diff --git a/Particle_Filter/GetRobotControl.m b/Particle_Filter/GetRobotControl.m deleted file mode 100644 index 8bbeac3..0000000 --- a/Particle_Filter/GetRobotControl.m +++ /dev/null @@ -1,6 +0,0 @@ -function u = GetRobotControl(k,Robot,it) -global nSteps; -u = [Robot.u(1,it)*(0.05); - Robot.u(1,it)*(0.05); - Robot.u(2,it)*(0.05)]; -end \ No newline at end of file diff --git a/Particle_Filter/MCL.m b/Particle_Filter/MCL.m deleted file mode 100644 index 4b52def..0000000 --- a/Particle_Filter/MCL.m +++ /dev/null @@ -1,103 +0,0 @@ -close all ; -clear all; - -global xTrue; -global Map; -global RTrue; -global UTrue; -global nSteps; -global maxDistance; - -nSteps = 2000; - -%change this to see how sensitive we are to the number of particle -%(hypotheses run) especially in relation to initial distribution ! -nParticles = 400; -Map = 140*rand(2,30)-70; %landmark position -UTrue = diag([0.01,0.01,1*pi/180]).^2; -RTrue = diag([2.0,3*pi/180]).^2; -UEst = 1.0*UTrue; -REst = 1.0*RTrue; -xTrue = [1;-40;-pi/2]; -maxDistance = 4; -xOdomLast = GetOdometry(1); - -%initial conditions: - a point cloud around truth -xP =repmat(xTrue,1,nParticles)+diag([8,8,0.4])*randn(3,nParticles); -%%%%%%%%% storage %%%%%%%% - -%initial graphics -figure(1); hold on; grid off ; axis equal; -plot(Map(1,:),Map(2,:),'g*');hold on; -set(gcf,'doublebuffer' , 'on'); -hObsLine = line ([0,0],[0,0]); -set(hObsLine,'linestyle',':'); -hPoints = plot(xP(1,:),xP(2,:), ' .' ); - -for k = 2:nSteps - %do world iteration and get also the new xTrue - SimulateWorld(k); - - %all particles are equally important - L = ones(nParticles,1)/nParticles; - - %figure out control - xOdomNow = GetOdometry(k); - u = tcomp(tinv(xOdomLast),xOdomNow); - xOdomLast = xOdomNow; - - %% do prediction - %for each particle we add in control vector AND noise - %the control noise adds diversity within the generation - for p = 1:nParticles - xP(:,p) = tcomp(xP(:,p),u+sqrt(UEst)*randn(3,1)); - end; - xP(3,:) = AngleWrapping(xP(3,:)); - - %% observe a randomn feature - [z,iFeature] = GetObservation(k); - if(~isempty(z)) - %predict observation - for(p = 1:nParticles) - %what do we expect observation to be for this particle ? - zPred = DoObservationModel(xP(:,p),iFeature,Map); - %on GetObs -> z = DoObservationModel(xTrue, iFeature,Map)+sqrt(RTrue)*randn(2,1); - % - %how different - Innov = z-zPred; - %get likelihood (new importance). Assume gaussian here but any pdf works! - %if predicted obs is very different from actual obs this score will be low - %->this particle is not very good at representing state . A lower score means - %it is less likely to be selected for the next generation ... - L(p) = exp(-0.5*Innov'*inv(REst)*Innov)+0.001; - end; - end; - - %% reselect based on weights: - %particles with big weights will occupy a greater percentage of the - %y axis in a cummulative plot - CDF = cumsum(L)/sum(L); - %so randomly (uniform) choosing y values is more likely to correspond to - %more likely (better) particles ... - iSelect = rand(nParticles,1); - %find the particle that corresponds to each y value (just a look up) - iNextGeneration = interp1(CDF,1:nParticles,iSelect,'nearest','extrap'); - - %copy selected particles for next generation .. - xP = xP(:,iNextGeneration); - %our estimate is simply the mean of teh particles - xEst = mean(xP,2); - if(mod(k-2,10)==0) - figure(1); - set(hPoints,'XData',xP(1,:)); - set(hPoints,'YData',xP(2,:)); - - if(~isempty(z)) - set(hObsLine,'XData',[xEst(1),Map(1,iFeature)]); - set(hObsLine,'YData',[xEst(2),Map(2,iFeature)]); - end; - - figure(2);plot(xP(1,:),xP(2,:), ' .' ); - drawnow; - end; -end; \ No newline at end of file diff --git a/Particle_Filter/PF.m b/Particle_Filter/PF.m deleted file mode 100644 index f51130e..0000000 --- a/Particle_Filter/PF.m +++ /dev/null @@ -1,122 +0,0 @@ -function [xEst,x_st,x_od] = PF(Robot,it,pMap,nParticles) -%PF Summary of this function goes here -% Detailed explanation goes here - - -global xTrue; -global Map; -global RTrue; -global UTrue; -global nSteps; -global LastOdom; - -nSteps = 8000; - -%change this to see how sensitive we are to the number of particle -%(hypotheses run) especially in relation to initial distribution ! -%nParticles = 400; -Map = max(pMap.points(1,:))*rand(2,30);% - (max(pMap.point(1,:))/2); %landmark position - -UTrue = diag([0.01,0.01,1*pi/180]).^2; - -RTrue = diag([0.7071,3*pi/180]).^2; - -UEst = 1.0*UTrue; - -REst = 1.0*RTrue; - -xTrue = Robot.q(it,:)'; - -xOdomLast = Robot.q(it,:)'; -x_st(it,1:3)= Robot.q(it,:)'; - -%initial conditions: - a point cloud around truth -xP =repmat(xTrue,1,nParticles)+diag([8,8,0.4])*randn(3,nParticles); -%%%%%%%%% storage %%%%%%%% - -%initial graphics -figure(101) -plot(Map(1,:),Map(2,:),'g*');hold on; -pMap.plotMap(); hold off - -figure(1); hold on; grid off ; axis equal; -plot(Map(1,:),Map(2,:),'g*');hold on; - -set(gcf,'doublebuffer' , 'on'); -hObsLine = line ([0,0],[0,0]); -set(hObsLine,'linestyle',':'); -pMap.plotMap(); -hPoints = plot(xP(1,:),xP(2,:), ' .' ); -k=1; - -for it = 2:nSteps - %do world iteration and get also the new xTrue - SimulateWorld(k,Robot,it); - - %all particles are equally important - L = ones(nParticles,1)/nParticles; - - %figure out control - xOdomNow = Robot.EKF_q_store(:,it); %GetOdometry(k,Robot,it); - u = tcomp(tinv(xOdomLast),xOdomNow); - xOdomLast = xOdomNow; - - %% do prediction - %for each particle we add in control vector AND noise - %the control noise adds diversity within the generation - for p = 1:nParticles - xP(:,p) = tcomp(xP(:,p),u+sqrt(UEst)*randn(3,1)); - end - xP(3,:) = AngleWrapping(xP(3,:)); - - %% observe a randomn feature - [z,iFeature] = GetObservation(it); - if(~isempty(z)) - %predict observation - for(p = 1:nParticles) - %what do we expect observation to be for this particle ? - zPred = DoObservationModel(xP(:,p),iFeature,Map); - %on GetObs -> z = DoObservationModel(xTrue, iFeature,Map)+sqrt(RTrue)*randn(2,1); - % - %how different - Innov = z-zPred; - %get likelihood (new importance). Assume gaussian here but any pdf works! - %if predicted obs is very different from actual obs this score will be low - %->this particle is not very good at representing state . A lower score means - %it is less likely to be selected for the next generation ... - L(p) = exp(-0.5*Innov'*inv(REst)*Innov)+0.001; - end; - end; - - %% reselect based on weights: - %particles with big weights will occupy a greater percentage of the - %y axis in a cummulative plot - CDF = cumsum(L)/sum(L); - %so randomly (uniform) choosing y values is more likely to correspond to - %more likely (better) particles ... - iSelect = rand(nParticles,1); - %find the particle that corresponds to each y value (just a look up) - iNextGeneration = interp1(CDF,1:nParticles,iSelect,'nearest','extrap'); - - %copy selected particles for next generation .. - xP = xP(:,iNextGeneration); - %our estimate is simply the mean of teh particles - xEst = mean(xP,2); -% if(mod(it-2,10)==0) -% figure(1); -% set(hPoints,'XData',xP(1,:)); -% set(hPoints,'YData',xP(2,:)); -% -% if(~isempty(z)) -% set(hObsLine,'XData',[xEst(1),Map(1,iFeature)]); -% set(hObsLine,'YData',[xEst(2),Map(2,iFeature)]); -% end; -% -% figure(2);plot(xP(1,:),xP(2,:), ' .' ); -% drawnow; -% end; -x_st(it,1:3)= xEst; -%x_od(it,1:3)= xOdomLast; -end; - -end % function \ No newline at end of file diff --git a/Particle_Filter/Particle.m b/Particle_Filter/Particle.m deleted file mode 100644 index 142ca81..0000000 --- a/Particle_Filter/Particle.m +++ /dev/null @@ -1,157 +0,0 @@ -%Monte-carlo based localisation -%note this is not coded efficiently but rather to make the ideas clear -%all loops should be vectorized but that gets a little matlab-speak intensive -%and may obliterate the elegance of a particle filter .... -function MCL -close all ; clear all; -global xTrue;global Map;global RTrue;global UTrue;global nSteps; -nSteps = 6000; -%change this to see how sensitive we are to the number of particle -%(hypotheses run) especially in relation to initial distribution ! -nParticles = 400; -Map = 140*rand(2,30)-70; -UTrue = diag([0.01,0.01,1*pi/180]).^2; -RTrue = diag([2.0,3*pi/180]).^2; -UEst = 1.0*UTrue; -REst = 1.0*RTrue; -xTrue = [1;-40;-pi/2]; -xOdomLast = GetOdometry(1); -%initial conditions: - a point cloud around truth -xP =repmat(xTrue,1,nParticles)+diag([8,8,0.4])*randn(3,nParticles); -%%%%%%%%% storage %%%%%%%% -%initial graphics -figure(1); hold on; grid off ; axis equal; -plot(Map(1,:),Map(2,:),'g*');hold on; -set(gcf,'doublebuffer' , 'on'); -hObsLine = line ([0,0],[0,0]); -set(hObsLine,'linestyle',':'); -hPoints = plot(xP(1,:),xP(2,:), ' .' ); -for k = 2:nSteps -%do world iteration -SimulateWorld(k); -%all particles are equally important -L = ones(nParticles,1)/nParticles; -%figure out control -xOdomNow = GetOdometry(k); -u = tcomp(tinv(xOdomLast),xOdomNow); -xOdomLast = xOdomNow; -%do prediction -%for each particle we add in control vector AND noise -%the control noise adds diversity within the generation -for(p = 1:nParticles) -xP(:,p) = tcomp(xP(:,p),u+sqrt(UEst)*randn(3,1)); -end; -xP(3,:) = AngleWrapping(xP(3,:)); -%observe a randomn feature -[z,iFeature] = GetObservation(k); -if(~isempty(z)) -%predict observation -for(p = 1:nParticles) -%what do we expect observation to be for this particle ? -zPred = DoObservationModel(xP(:,p),iFeature,Map); -%how different -Innov = z-zPred; -%get likelihood (new importance). Assume gaussian here but any pdf works! -%if predicted obs is very different from actual obs this score will be low -%->this particle is not very good at representing state . A lower score means -%it is less likely to be selected for the next generation ... -L(p) = exp(-0.5*Innov'*inv(REst)*Innov)+0.001; -end; -end; -%reselect based on weights: -%particles with big weights will occupy a greater percentage of the -%y axis in a cummulative plot -CDF = cumsum(L)/sum(L); -%so randomly (uniform) choosing y values is more likely to correspond to -%more likely (better) particles ... -iSelect = rand(nParticles,1); -%find the particle that corresponds to each y value (just a look up) -iNextGeneration = interp1(CDF,1:nParticles,iSelect,'nearest','extrap'); -%copy selected particles for next generation .. -xP = xP(:,iNextGeneration); -%our estimate is simply the mean of teh particles -xEst = mean(xP,2); -if(mod(k-2,10)==0) -figure(1); -set(hPoints,'XData',xP(1,:)); -set(hPoints,'YData',xP(2,:)); -if(~isempty(z)) -set(hObsLine,'XData',[xEst(1),Map(1,iFeature)]); -set(hObsLine,'YData',[xEst(2),Map(2,iFeature)]); -end; -figure(2);plot(xP(1,:),xP(2,:), ' .' ); -drawnow; -end; -end; -end -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -function [z,iFeature] = GetObservation(k) -global Map;global xTrue;global RTrue;global nSteps; -%fake sensor failure here -if(abs(k-nSteps/2)<0.1*nSteps) -z = []; -iFeature = -1; -else -iFeature = ceil(size(Map,2)*rand(1)); -z = DoObservationModel(xTrue, iFeature,Map)+sqrt(RTrue)*randn(2,1); -z(2) = AngleWrapping(z(2)); -end; -end -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -function [z] = DoObservationModel(xVeh, iFeature,Map) -Delta = Map(1:2,iFeature)-xVeh(1:2); -z = [norm(Delta); -atan2(Delta(2),Delta(1))-xVeh(3)]; -z(2) = AngleWrapping(z(2)); -end -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -function SimulateWorld(k) -global xTrue; -u = GetRobotControl(k); -xTrue = tcomp(xTrue,u); -xTrue(3) = AngleWrapping(xTrue(3)); -end -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -function [xnow] = GetOdometry(k) -persistent LastOdom; %internal to robot low-level controller -global UTrue; -if(isempty(LastOdom)) -global xTrue; -LastOdom = xTrue; -end; -u = GetRobotControl(k); -xnow =tcomp(LastOdom,u); -uNoise = sqrt(UTrue)*randn(3,1); -xnow = tcomp(xnow,uNoise); -LastOdom = xnow; -end -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -function u = GetRobotControl(k) -global nSteps; -u = [0; 0.025 ; 0.1* pi/180*sin(3*pi*k/nSteps)]; -end -function tac = tcomp(tab,tbc) - - -% final angle -result = tab(3)+tbc(3); % add the angles - -if result > pi | result <= -pi - result = AngleWrapping(result) ; -end - -s = sin(tab(3)); -c = cos(tab(3)); - - % actual value added with the new control vector - tac = [tab(1:2)+ [c -s; s c]*tbc(1:2); - result]; -end -function angle = AngleWrapping(angle) - -if(angle>pi) - angle=angle-2*pi; -elseif(angle<-pi) - angle = angle+2*pi; -end; -end \ No newline at end of file diff --git a/Particle_Filter/SimulateWorld.m b/Particle_Filter/SimulateWorld.m deleted file mode 100644 index e8d8acf..0000000 --- a/Particle_Filter/SimulateWorld.m +++ /dev/null @@ -1,6 +0,0 @@ -function SimulateWorld(k,Robot,it) -global xTrue; -u = GetRobotControl(k,Robot,it); -xTrue = Robot.q(it,:)'; -xTrue(3) = AngleWrapping(xTrue(3)); -end \ No newline at end of file diff --git a/Particle_Filter/tcomp.m b/Particle_Filter/tcomp.m deleted file mode 100644 index dd764f5..0000000 --- a/Particle_Filter/tcomp.m +++ /dev/null @@ -1,18 +0,0 @@ -function tac = tcomp(tab,tbc) - - -% final angle -result = tab(3)+tbc(3); % add the angles of the input tbc to state tab - -%we check to avoid discontinuity -if result > pi || result <= -pi - result = AngleWrapping(result) ; -end - -s = sin(tab(3)); -c = cos(tab(3)); - - % actual value added with the new control vector - tac = [tab(1:2)+ [c -s; s c]*tbc(1:2); - result]; -end \ No newline at end of file diff --git a/Particle_Filter/tinv.m b/Particle_Filter/tinv.m deleted file mode 100644 index 8d4b0b9..0000000 --- a/Particle_Filter/tinv.m +++ /dev/null @@ -1,8 +0,0 @@ -function tba=tinv(tab) - -tba = zeros(size(tab)); -for t=1:3:size(tab,1), - tba(t:t+2) = tinv1(tab(t:t+2)); -end -end - diff --git a/Particle_Filter/tinv1.m b/Particle_Filter/tinv1.m deleted file mode 100644 index 4a2b3c1..0000000 --- a/Particle_Filter/tinv1.m +++ /dev/null @@ -1,11 +0,0 @@ -%------------------------------------------------------- -function tba=tinv1(tab) -% -% calculates the inverse of one transformations -%------------------------------------------------------- -s = sin(tab(3)); -c = cos(tab(3)); -tba = [-tab(1)*c - tab(2)*s; - tab(1)*s - tab(2)*c; - -tab(3)]; -end \ No newline at end of file diff --git a/Particle_filter_test.m b/Particle_filter_test.m deleted file mode 100644 index 6afee95..0000000 --- a/Particle_filter_test.m +++ /dev/null @@ -1,24 +0,0 @@ -addpath('Particle_Filter') -close all -%clc -%load datapff.mat -%load test_pf_offline.mat -[xest_a,xa_st] = PF(robot{1,1},1,map,2000); -[xest_b,xb_st] = PF(robot{1,2},1,map,2000); -[xest_c,xc_st] = PF(robot{1,3},1,map,2000); - -figure(90); axis equal; -hold on -plot(robot{1,1}.q(:,1),robot{1,1}.q(:,2),'r') -plot(xa_st(:,1),xa_st(:,2),'b') - - -figure(91); axis equal; -hold on -plot(robot{1,2}.q(:,1),robot{1,2}.q(:,2),'r') -plot(xb_st(:,1),xb_st(:,2),'b') - -figure(92); axis equal; -hold on -plot(robot{1,3}.q(:,1),robot{1,3}.q(:,2),'r') -plot(xc_st(:,1),xc_st(:,2),'b') diff --git a/README.md b/README.md index 515442b..63c136e 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,21 @@ # Distributed Multirobot Exploration and Mapping -project for final exam +Project for the final exam in this paper, we consider the problem of exploring an environment +unknown with a team of robots. +As in the exploration of single robots, the goal is to minimize the +overall exploration time. +The key problem to solve in the context of multiple robots is that of +choose the appropriate destination points for the individual robots so that +can explore different regions of the environment simultaneously. We present +an approach for the coordination of multiple robots, which takes into account simultaneously +of the cost of reaching a target point and its usefulness. +We also describe how our algorithm can be extended to situations +in which the communication range of the robots is limited. +The filter was used to estimate the positions of the robots +particle, assuming a communication with anchors Wi-Fi. +The results show that our technique effectively distributes the robots +on the environment and allows them to fulfill their mission quickly. ## Installing @@ -12,9 +26,9 @@ Depending on the machine you are using: - Linux First delete files with extensions \*.mex*architecturename*, for example, for Mac the file will have extension: ***.mexmaci64** -you need to compile some mex files before starting: +you need to compile some mex files before starting: - **sens\_model\_noise.c** -- **potentialfiled.cpp** +- **potentialfiled.cpp** - **mapgen.cpp** type in terminal: @@ -29,46 +43,120 @@ Now you can run the script. If you encounter problems in compiling using the mex command, update Matlab to the latest available release or refer to the [official site](https://mathworks.com/). ## Run -To launch the script, open the *Start.m* file. +To launch the script, open the *Main.m* and press "Run", you can customize the simulation by acting on the parameters as below (used in last simulation): +```Matlab +% define number of robots to use +numrobot = 4; +% define base time +basetime = 200; +% increment of time +k = 9; +``` +For a longer simulation we recommend running the ruby script and following the instructions on the screen +```bash +$ ruby runsimulation.rb +``` + +Launch script, open the *Demo.m* and press "Run" to view plot and animation. + +## Report -### How to use Robot class +The report is available [here](https://github.com/frank1789/DistributedSystemProject/blob/master/Report/Report.pdf) or in the **"Report"** folder. + +## How to use + +**Main.m** ```Matlab -% Generating map -% build a new map with map = Map("new", widht, height) -% advice: do not exceed 100 x 100 size -% or load an existing one map = Map("load") -map = Map('load'); -figure(800); axis equal +%% Main - Start multirobot +close all +clear +clc +addpath('Utility-Mapping') +compilemexlibrary +%% Generate Map +% build a new map with: +% map = Map("New", width, heigth); +% map = Map("New", width, heigth, #landmark, "auto"); +% map = Map("New", width, heigth, #landmark, "manual"); +% or load an existing one: +% map = Map("Load"); +% map = Map("Load", #landamark); +% map = Map("Load", #landamark, "auto"); +% map = Map("Load", #landamark, "manual"); +map = Map('new', 100, 100); +figure('units','normalized','outerposition',[0 0 1 1]); axis equal +axis([0, 100, 0, 100]) map.plotMap(); +print('map100x100','-depsc','-r0') +%% Set-up paramaters simulation +% define number of robots to use +numrobot = 4; +% define base time +basetime = 200; +% increment of time +k = 9; + +parfor (k = 1:7, 4) + for n = 1:5 + multirobot(n, k * 150, map, 1) + end +end +%% analysis result +Result +``` +## Use of classes -%% set-up simulation parameters +### Robot class +```Matlab +% set-up simulation parameters % Sampling time MdlInit.Ts = 0.05; % Length of simulation MdlInit.T = 25; time = 0:MdlInit.Ts:MdlInit.T; -%% Vehicle set-up initial conditions +% Vehicle set-up initial conditions robot = Robot(1, MdlInit.T, MdlInit.Ts, map.getAvailablePoints()); % set 1st target robot.setpointtarget(map.getAvailablePoints()); -%% Perfrom simulation -w = waitbar(0,'Please wait simulation in progress...'); +% Perfrom simulation for indextime = 1:1:length(time) if mod(indextime,2) == 0 % simualte laserscan @ 10Hz robot.scanenvironment(map.points, map.lines, indextime); end robot.UnicycleKinematicMatlab(indextime); - robot.ekfslam(indextime); - - waitbar(indextime/length(time), w, ... - sprintf('Please wait simulation in progress... %3.2f%%',... - indextime/length(time) * 100)); end -close(w); clear w; % delete ui ``` +### Map class +```Matlab +% Generate Map +% build a new map with: +% map = Map("New", width, heigth); +% map = Map("New", width, heigth, #landmark, "auto"); +% map = Map("New", width, heigth, #landmark, "manual"); +% or load an existing one: +% map = Map("Load"); +% map = Map("Load", #landamark); +% map = Map("Load", #landamark, "auto"); +% map = Map("Load", #landamark, "manual"); +% example +map = Map('new', 100, 100); +map.plotMap(); +``` + +### Particle filter class + +Within the main cycle after the calculation of the kinematic model insert +```Matlab +if indextime == 1 + pf = Particle_Filter(robot, map.landmark, indextime); +else + pf{i} = pf{i}.update(robot, indextime); + robot = robot.setParticleFilterxEst(pf.xEst); +end +``` ## Authors - Argentieri Francesco diff --git a/Report/Report.pdf b/Report/Report.pdf index c634e64..f3cf923 100644 Binary files a/Report/Report.pdf and b/Report/Report.pdf differ diff --git a/StartMultirobot_3_correct.m b/StartMultirobot_3_correct.m deleted file mode 100644 index 2c95bcf..0000000 --- a/StartMultirobot_3_correct.m +++ /dev/null @@ -1,298 +0,0 @@ -%% Main file -close all; -clear all; -clear class; -clc; -addpath ('Cost Function', 'Occupacy grid from Image') -% diary log.txt -% load map -MapName = 'map_square.mat'; -mapStuct = load( MapName ); -mapStuct.map.points = [[0 16 16 0]; [0 0 16 16]]; -% more wall -mapStuct.map.points = [ mapStuct.map.points, [12 12; 12 10],[12 12; 14 16],[13 6; 10 10],[4 4; 4 0],[4 8; 4 4]]; -mapStuct.map.lines = [mapStuct.map.lines,[5;6], [7;8], [9;10], [11;12], [13;14]]; -figure(800) -hold on -plotMap(mapStuct.map); -hold off -axis equal -grid on -pause(1); close(figure(800)); -p=1; - -%% simulation -% Sampling time -MdlInit.Ts = 0.05; -% Length of simulation -MdlInit.T = 800; -nit = 0:MdlInit.Ts:MdlInit.T; -% Vehicle set-up Vehicle initial conditions -Vehicle.q{1} = [2, 2, 0]; -% Vehicle.q{2} = [1 1; pi]; -% Vehicle.q{3} = [-7; 3; 0]; -a = Robot(1, MdlInit.T, MdlInit.Ts, Vehicle.q{p}); % istance robot - -theta=-pi/2:0.36*pi/180:pi/2; -ris = 0.15; -r0=4/ris; - -y=r0*sin(theta); -x=r0*cos(theta); - -x_g=floor(x); -y_g=floor(y); - -max_x = max(x_g); -max_y = max(y_g); - -n_mis = 1000;%length(laserScan_xy); - -occ_mat = zeros(max_x,2*max_y); -lid_mat = zeros(max_x,2*max_y); - -room_length = 16; -room_width = 16; - -lgth = floor(room_length/ris); -wdt = floor(room_width/ris); - -Global_map = zeros(lgth,wdt,3); -Cost_map = zeros(lgth-1,wdt-1,3); - -center_x = floor(lgth/2); -center_y = floor(wdt/2); - -laserScan_xy = cell.empty; -j=1; -kk=1; -zz=1; - -A = zeros(3); -front = zeros(2); - -% use this method to set target -a.setpointtarget([13 13 0]); - -w = waitbar(0,'Please wait simulation in progress...'); - -tic -steps = length(nit); -% Vehicle set-up initial conditions -Vehicle.q{1} = [1 1 0]; -Vehicle.q{2} = [5 5 4/5*pi]; -Vehicle.q{3} = [6 1 -pi/4]; -robot = cell.empty; -for jj = 1:3 - robot{jj} = Robot(jj, MdlInit.T, MdlInit.Ts, Vehicle.q{jj}); -end - - -robot{1}.setpointtarget([1,15,0]); -robot{2}.setpointtarget([15,15,0]); -robot{3}.setpointtarget([10,12,0]); - -for ii = 1:1:length(nit) -for rr=1:1:3 - if mod(ii,2) == 0 % simualte laserscan @ 10Hz - robot{rr}.scanenvironment(mapStuct.map.points, mapStuct.map.lines, ii); - end - robot{rr}.UnicycleKinematicMatlab(ii); - - - if mod(ii,10)==0 - if(isempty(robot{rr}.laserScan_2_xy{ii}) || all((all(isnan(robot{rr}.laserScan_2_xy{ii})))==1)) - - else - out = robot{rr}.laserScan_2_xy{ii}(:,all(~isnan(robot{rr}.laserScan_2_xy{ii}))); - [ occ_mat(:,:)] = Occ_Grid( occ_mat(:,:),lid_mat(:,:),out); - - - - for i = 1:1:length(occ_mat(:,1)) - for j = 1:1:length(occ_mat(1,:)) - - if mod(ii,40)==0 - - % Compute Cost matrix - A = [cos(robot{rr}.q(ii,3)), -sin(robot{rr}.q(ii,3)), robot{rr}.q(ii,1)/ris; - sin(robot{rr}.q(ii,3)), cos(robot{rr}.q(ii,3)), robot{rr}.q(ii,2)/ris; - 0 0 1]*[i ;(length(occ_mat(1,:)))/2+1-j; 1]; - A=floor(A); - if(A(2)>wdt || A(2)==wdt) - A(2) = wdt-1; - end - if(A(2)==0 || A(2)<0) - A(2)=1; - end - if(A(1)==0 || A(1)<0) - A(1)=1; - end - if(A(1)>lgth) - A(1) = lgth; - end - CC=[A(2),A(1)]; - Cost_map(wdt-CC(1),CC(2))= 5;%min(10,Cost_map(wdt-CC(1),CC(2)) +10/(i^2+j^2-2)^0.25); - - %Reset Target Location - - - y=0.15*r0*sin(theta+robot{rr}.q(ii,3))+robot{rr}.q(ii,2); - x=0.15*r0*cos(theta+robot{rr}.q(ii,3))+robot{rr}.q(ii,1); - front = [x;y]; - - for ll = 1:1:250 - if(isnan(robot{rr}.laserScan_2_xy{1,ii}(1,251-ll)) && front(1,251-ll)>0 && front(2,251-ll)>0) - robot{rr}.setpointtarget([front(1,251-ll) front(2,251-ll) 0]); - break - else if(isnan(robot{rr}.laserScan_2_xy{1,ii}(1,251+ll))&& front(1,251+ll)>0 && front(2,251+ll)>0) - robot{rr}.setpointtarget([front(1,251+ll) front(2,251+ll) 0]); - break - end - end - end - - if mod(ii,160)==0 - [raw,column] = minmat(Cost_map); - new_target =[ceil(column*0.15);ceil(17-raw*0.15);1]; - robot{rr}.setpointtarget([new_target(1) new_target(2) 0]); - end - end - - end - end - end - kk=kk+1; - waitbar(ii/steps,w,sprintf('Please wait simulation in progress... %3.2f%%',ii/steps *100)) - end - if mod(ii,2)==0 -Global_map(:,:,rr) = Update_gbmap(Global_map(:,:,rr),robot{rr},ii,wdt,lgth,occ_mat,lid_mat); - end - end -end -close(w) -diary off - -% for i = 2:a.getEKFstep() -% a.prediction(i); -% a.update(i); -% a.store(i); -% end -toc - -%% Animation -% pre-allocating for speed -body = cell.empty; -label = cell.empty; -rf = cell.empty; -rf_x= cell.empty; -rf_y= cell.empty; -rf_z= cell.empty; -% setup figure -figure(); -for n= 1:30:length(a.t) - title(['Time: ', num2str(a.t(n),5)]) - axis([0, 18, 0, 18]); hold on; axis equal; grid on; - hold on - plotMap(mapStuct.map); - fa = quiver(a.q(n,1), a.q(n,2),a.target(1)-a.q(n,1), a.target(2)-a.q(n,2),'c'); - plot(a.target(1), a.target(2), '*r') - plot(a.q(:,1), a.q(:,2), 'g-.') - for j=1:1 - if n == 1 - [body{j}, label{j}, rf_x{j}, rf_y{j}, rf_z{j}] =a.makerobot(n); - else - delete([body{j}, label{j}, rf_x{j}, rf_y{j}, rf_z{j}]); - [body{j}, label{j}, rf_x{j}, rf_y{j}, rf_z{j}] =a.animate(n); - end - drawnow; - end - hold off - % local variable cluodpoint - cloudpoint = (a.getlaserscan(n)); - % verify cloudpoint is nonvoid vector - if ~isempty(cloudpoint) - cl_point = plot(cloudpoint(1,:),cloudpoint(2,:),'.b'); % plot - end - axis equal - grid on -end % end animation -% -% -% for i=1:1:j-1 -% figure -% mesh(occ_mat(:,:,i)); -% end - -figure -mesh(Global_map) -figure -mesh(Cost_map) - -%save test.mat - - -% -% -% %% Initial Condition -% n=1; -% title(['Time: ', num2str(a.t(n),5)]) -% axis([0, 18, 0, 18]); hold on; axis equal; grid on; -% hold on -% plotMap(mapStuct.map); -% fa = quiver(a.q(n,1), a.q(n,2),a.target(1)-a.q(n,1), a.target(2)-a.q(n,2),'c'); -% plot(a.target(1), a.target(2), '*r') -% plot(a.q(:,1), a.q(:,2), 'g-.') -% for j=1:1 -% if n == 1 -% [body{j}, label{j}, rf_x{j}, rf_y{j}, rf_z{j}] =a.makerobot(n); -% else -% delete([body{j}, label{j}, rf_x{j}, rf_y{j}, rf_z{j}]); -% [body{j}, label{j}, rf_x{j}, rf_y{j}, rf_z{j}] =a.animate(n); -% end -% drawnow; -% end -% hold off -% % local variable cluodpoint -% cloudpoint = (a.getlaserscan(n)); -% % verify cloudpoint is nonvoid vector -% if ~isempty(cloudpoint) -% cl_point = plot(cloudpoint(1,:),cloudpoint(2,:),'.b'); % plot -% end -% axis equal -% grid on -% -% - - - - - -% -% -% %Update Global Map -% if(occ_mat(i,j)~=0) -% A = [cos(robot{rr}.q(ii,3)), -sin(robot{rr}.q(ii,3)), robot{rr}.q(ii,1)/ris; -% sin(robot{rr}.q(ii,3)), cos(robot{rr}.q(ii,3)), robot{rr}.q(ii,2)/ris; -% 0 0 1]*[i ;(length(occ_mat(1,:)))/2+1-j; 1]; -% A=floor(A); -% if(A(2)>wdt || A(2)==wdt) -% A(2) = wdt-1; -% end -% if(A(2)==0 || A(2)<0) -% A(2)=1; -% end -% if(A(1)==0 || A(1)<0) -% A(1)=1; -% end -% if(A(1)>lgth) -% A(1) = lgth; -% end -% CC=[A(2),A(1)]; -% if(Global_map(wdt-CC(1),CC(2))==0) -% Global_map(wdt-CC(1),CC(2),rr)= occ_mat(i,j); -% else -% Global_map(wdt-CC(1),CC(2),rr)= (occ_mat(i,j) + Global_map(wdt-CC(1),CC(2),rr))/2; -% end -% end -% \ No newline at end of file diff --git a/Start_6.m b/Start_6.m deleted file mode 100644 index 3fa18cf..0000000 --- a/Start_6.m +++ /dev/null @@ -1,215 +0,0 @@ -%% Main - Start multirobot -close all -clear class -clear -clc -addpath('Utility-Mapping') -%% Generating map -% build a new map with map = Map("new",widht,height); -% or load an existing one map = Map("load") -map = Map('load'); -figure(800); axis equal -map.plotMap(); -% time sample -MdlInit.Ts = 0.05; -% Length of simulation -MdlInit.T = 200; -p=1; - -%% simulation -% Sampling time -MdlInit.Ts = 0.05; -% Length of simulation -MdlInit.T = 100; -nit = 0:MdlInit.Ts:MdlInit.T; -% Vehicle set-up Vehicle initial conditions -Vehicle.q{1} = [2, 2, 0]; - -a = Robot(1, MdlInit.T, MdlInit.Ts, Vehicle.q{p}); % istance robot - -theta=-pi/2:0.36*pi/180:pi/2; -ris = 0.15; -r0=4/ris; - -y=r0*sin(theta); -x=r0*cos(theta); - -x_g=floor(x); -y_g=floor(y); - -max_x = max(x_g); -max_y = max(y_g); - -n_mis = 1000;%length(laserScan_xy); - -occ_mat = zeros(max_x,2*max_y); -lid_mat = zeros(max_x,2*max_y); - -room_length = 16; -room_width = 16; - -lgth = floor(room_length/ris); -wdt = floor(room_width/ris); - -Global_map = zeros(lgth,wdt); -Cost_map = zeros(lgth-1,wdt-1); - -center_x = floor(lgth/2); -center_y = floor(wdt/2); - -laserScan_xy = cell.empty; -j=1; -kk=1; -zz=1; - -A = zeros(3); -front = zeros(2); - -% use this method to set target -a.setpointtarget([10 14 0]); - -w = waitbar(0,'Please wait simulation in progress...'); - -tic -steps = length(nit); -for ii = 1:1:length(nit) - if mod(ii,2) == 0 % simualte laserscan @ 10Hz - a.scanenvironment(map.points, map.lines, ii); - end - a.UnicycleKinematicMatlab(ii); - a.ekfslam(ii); - % if ii>=2 - % [pose, Cx0, Ik, Sw, w1, w2, ell_angle] = odometriclocalization(a, ii, pose, Ik, Cx0, Cw, Sw); - % end -% if mod(ii,10)==0 -% laserScan_xy{ii} = a.laserScan_2_xy(ii); -% -% if(isempty(laserScan_xy{1,ii}{1,1}) || all((all(isnan(laserScan_xy{1,ii}{1,1})))==1)) -% -% else -% out = laserScan_xy{1,ii}{1,1}(:,all(~isnan(laserScan_xy{1,ii}{1,1}))); -% [ occ_mat(:,:)] = Occ_Grid( occ_mat(:,:),lid_mat(:,:),out); -% -% -% -% for i = 1:1:length(occ_mat(:,1)) -% for j = 1:1:length(occ_mat(1,:)) -% -% if mod(ii,40)==0 -% -% % Compute Cost matrix -% A = [cos(a.q(ii,3)), -sin(a.q(ii,3)), a.q(ii,1)/ris; -% sin(a.q(ii,3)), cos(a.q(ii,3)), a.q(ii,2)/ris; -% 0 0 1]*[i ;(length(occ_mat(1,:)))/2+1-j; 1]; -% A=floor(A); -% if(A(2)>wdt || A(2)==wdt) -% A(2) = wdt-1; -% end -% if(A(2)==0 || A(2)<0) -% A(2)=1; -% end -% if(A(1)==0 || A(1)<0) -% A(1)=1; -% end -% if(A(1)>lgth) -% A(1) = lgth; -% end -% CC=[A(2),A(1)]; -% Cost_map(wdt-CC(1),CC(2))= 5;%min(10,Cost_map(wdt-CC(1),CC(2)) +10/(i^2+j^2-2)^0.25); -% -% %Reset Target Location -% -% -% y=0.15*r0*sin(theta+a.q(ii,3))+a.q(ii,2); -% x=0.15*r0*cos(theta+a.q(ii,3))+a.q(ii,1); -% front = [x;y]; -% -% for ll = 1:1:250 -% if(isnan(a.laserScan_2_xy{1,ii}(1,251-ll)) && front(1,251-ll)>0 && front(2,251-ll)>0) -% a.setpointtarget([front(1,251-ll) front(2,251-ll) 0]); -% break -% else if(isnan(a.laserScan_2_xy{1,ii}(1,251+ll))&& front(1,251+ll)>0 && front(2,251+ll)>0) -% a.setpointtarget([front(1,251+ll) front(2,251+ll) 0]); -% break -% end -% end -% end -% -% if mod(ii,160)==0 -% [raw,column] = minmat(Cost_map); -% new_target =[ceil(column*0.15);ceil(17-raw*0.15);1]; -% a.setpointtarget([new_target(1) new_target(2) 0]); -% end -% end -% %Update Global Map -% if(occ_mat(i,j)~=0) -% A = [cos(a.q(ii,3)), -sin(a.q(ii,3)), a.q(ii,1)/ris; -% sin(a.q(ii,3)), cos(a.q(ii,3)), a.q(ii,2)/ris; -% 0 0 1]*[i ;(length(occ_mat(1,:)))/2+1-j; 1]; -% A=floor(A); -% if(A(2)>wdt || A(2)==wdt) -% A(2) = wdt-1; -% end -% if(A(2)==0 || A(2)<0) -% A(2)=1; -% end -% if(A(1)==0 || A(1)<0) -% A(1)=1; -% end -% if(A(1)>lgth) -% A(1) = lgth; -% end -% CC=[A(2),A(1)]; -% if(Global_map(wdt-CC(1),CC(2))==0) -% Global_map(wdt-CC(1),CC(2))= occ_mat(i,j); -% else -% Global_map(wdt-CC(1),CC(2))= (occ_mat(i,j) + Global_map(wdt-CC(1),CC(2)))/2; -% end -% end -% -% end -% end -% end -% kk=kk+1; -% waitbar(ii/steps,w,sprintf('Please wait simulation in progress... %3.2f%%',ii/steps *100)) -% end - -end -close(w) -%% Animation -% pre-allocating for speed -body = cell.empty; -label = cell.empty; -rf = cell.empty; -rf_x= cell.empty; -rf_y= cell.empty; -rf_z= cell.empty; -% setup figure -figure(); -for n= 1:30:length(a.t) - title(['Time: ', num2str(a.t(n),5)]) - axis([0, 18, 0, 18]); hold on; axis equal; grid on; - hold on - map.plotMap(); - fa = quiver(a.q(n,1), a.q(n,2),a.target(1)-a.q(n,1), a.target(2)-a.q(n,2),'c'); - plot(a.target(1), a.target(2), '*r') - plot(a.q(:,1), a.q(:,2), 'g-.') - for j=1:1 - if n == 1 - [body{j}, label{j}, rf_x{j}, rf_y{j}, rf_z{j}] =a.makerobot(n); - else - delete([body{j}, label{j}, rf_x{j}, rf_y{j}, rf_z{j}]); - [body{j}, label{j}, rf_x{j}, rf_y{j}, rf_z{j}] =a.animate(n); - end - drawnow; - end - hold off - % local variable cluodpoint - cloudpoint = (a.getlaserscan(n)); - % verify cloudpoint is nonvoid vector - if ~isempty(cloudpoint) - cl_point = plot(cloudpoint(1,:),cloudpoint(2,:),'.b'); % plot - end - axis equal - grid on -end % end animation diff --git a/StartMultirobot_4.m b/demo.m similarity index 52% rename from StartMultirobot_4.m rename to demo.m index ca0d589..724e7f3 100644 --- a/StartMultirobot_4.m +++ b/demo.m @@ -1,7 +1,6 @@ %% Main - Start multirobot close all - -clearvars -except map +clear clc addpath('Utility-Mapping') @@ -15,19 +14,13 @@ % map = Map("Load", #landamark); % map = Map("Load", #landamark, "auto"); % map = Map("Load", #landamark, "manual"); -map = Map('new',40,40); -%0figure(801); axis equal -%map.plotMap(); +map = Map('new', 40, 40); +figure(801); axis equal +map.plotMap(); % time sample MdlInit.Ts = 0.05; % Length of simulation -MdlInit.T = 1200; - -%cost parameter -beta=0.5; -itneeded =0; -comunication = 0; - +MdlInit.T = 120; nit = MdlInit.T / MdlInit.Ts; %Total application iteration % preallocate @@ -35,16 +28,15 @@ occparameters = cell.empty; pf = cell.empty; -% Vehicle set-up initial conditions jj = 1:3 -for jj = 1:1 - %initialize robot and destination +% Vehicle set-up initial conditions +for jj = 1:3 + % initialize robot and destination robot{jj} = Robot(jj, MdlInit.T, MdlInit.Ts, map.getAvailablePoints(),map,0.15); robot{jj} = robot{jj}.setpointtarget(map.getAvailablePoints()); - %initialize parameters for occupacy & cost function + % initialize parameters for occupacy & cost function [ occparameters{jj} ] = cinitialize(robot{jj}, map, nit, 0.15); end -%% Online Simulation of all 3 Robot w = waitbar(0,'Please wait simulation in progress...'); for ii = 1:1:nit for i = 1:1:length(robot) @@ -52,7 +44,6 @@ robot{i} = robot{i}.scanenvironment(map.points, map.lines, ii); end robot{i} = robot{i}.UnicycleKinematicMatlab(ii); - % robot{i} = robot{i}.ekfslam(ii); if ii == 1 pf{i} = Particle_Filter(robot{i}, map.landmark, ii); else @@ -65,33 +56,26 @@ occparameters{rr}.already_visit = 0; %If lidar information is avaible update Global Map of each robot if mod(ii,20) == 0 %Update Global & Cost Map 1 Hz every 1s ii =20 - if ii > 300 %Update Global map Update_gbmap(robot{rr},ii,occparameters{rr}); end - if (occparameters{rr}.already_visit && ii> occparameters{rr}.it_needed) % set target in base of visibility matrix - + if (occparameters{rr}.already_visit && ii> occparameters{rr}.it_needed) % set target in base of visibility matrix [itneeded,target] = Reset_Main_Target(robot{rr},ii,occparameters{rr}); - fprintf('aggiorno il target sulla mappa iterazione: %5i\n', robot{rr}.target) - fprintf('it needed: %5i\n', itneeded) occparameters{rr}.it_needed = itneeded +ii; - robot{rr}.setpointtarget(target); - - elseif (ii> occparameters{rr}.it_needed) + robot{rr} = robot{rr}.setpointtarget(target); + elseif (ii > occparameters{rr}.it_needed) robot{rr}.setpointtarget(Reset_Target_2(robot{rr},ii, occparameters{rr})); end - - if mod(ii,40)==0 % ii = 40 - fprintf('aggiorno cost_map %5i\n', ii); + if mod(ii,40) == 0 % ii = 40 %Update visibility Matrix - occparameters{rr}.Cost_map(:,:) = Update_vis(occparameters{rr},robot{rr},ii); %ToDo da rivedere + occparameters{rr}.Cost_map(:,:) = Update_vis(occparameters{rr},robot{rr},ii); end end - if (mod(ii,2) == 0 && ~occparameters{rr}.comunication) % dare un intervallo che non lo faccia ripetere subito dopo + if (mod(ii,2) == 0 && ~occparameters{rr}.comunication) [occparameters] = comunicate(robot,ii,rr,occparameters); end % Reset Comunication Parameter when a given temporal delay is overcame @@ -108,48 +92,44 @@ end close(w); clear w; -% %% Animation -% % pre-allocating for speed -% body = cell.empty; -% label = cell.empty; -% rf = cell.empty; -% rf_x= cell.empty; -% rf_y= cell.empty; -% rf_z= cell.empty; -% cl_point = cell.empty; -% cloudpoint = cell.empty; -% % setup figure -% figure(); hold on; -% for n= 1:length(robot{1}.t) -% title(['Time: ', num2str(robot{1}.t(n),5)]) -% hold on; -% axis equal; grid on; -% -% for j = 1:1:length(robot) -% plot(robot{j}.target(1), robot{j}.target(2), '*r') -% plot(robot{j}.q(:,1), robot{j}.q(:,2), 'g-.') -% if n == 1 -% [body{j}, label{j}, rf_x{j}, rf_y{j}, rf_z{j}] = robot{j}.makerobot(n); -% else -% delete([body{j}, label{j}, rf_x{j}, rf_y{j}, rf_z{j}]); -% [body{j}, label{j}, rf_x{j}, rf_y{j}, rf_z{j}] = robot{j}.animate(n); -% end -% -% % cloudpoint{j} = drawscan(robot{j},pf{j}, n); % local variable cluodpoint -% % if ~isempty(cloudpoint{j}) % verify cloudpoint is nonvoid vector -% % [cl_point{j}] = plot(cloudpoint{j}(1,:),cloudpoint{j}(2,:),'.b'); % plot -% % end -% end -% drawnow; -% hold off -% end % animation -% hold off -% -% %% check trajectories -% for z = 1:length(robot) -% figure(); hold on; axis equal; -% plot(robot{z}.q(:,1),robot{z}.q(:,2),'b') -% %plot(robot{z}.EKF_q_store(1,:),robot{z}.EKF_q_store(2,:)) -% plot(pf{z}.xEst(:,1),pf{z}.xEst(:,2),'r') -% hold off -% end \ No newline at end of file +%% Animation +% pre-allocating for speed +body = cell.empty; +label = cell.empty; +rf_x= cell.empty; +rf_y= cell.empty; +rf_z= cell.empty; +cl_point = cell.empty; +cloudpoint = cell.empty; +% setup figure +figure(); hold on; +for n= 1:nit + title(['Time: ', num2str(robot{1}.t(n),5)]) + hold on; + axis([0 map.width 0 map.height]); grid on; + for j = 1:1:length(robot) + plot(robot{j}.target(1), robot{j}.target(2), '*r') + plot(robot{j}.q(:,1), robot{j}.q(:,2), 'g-.') + if n == 1 + [body{j}, label{j}, rf_x{j}, rf_y{j}, rf_z{j}] = robot{j}.makerobot(n); + else + delete([body{j}, label{j}, rf_x{j}, rf_y{j}, rf_z{j}]); + [body{j}, label{j}, rf_x{j}, rf_y{j}, rf_z{j}] = robot{j}.animate(n); + end + cloudpoint{j} = drawscan(robot{j},pf{j}, n); % local variable cluodpoint + if ~isempty(cloudpoint{j}) % verify cloudpoint is nonvoid vector + [cl_point{j}] = plot(cloudpoint{j}(1,:),cloudpoint{j}(2,:),'.b'); % plot + end + end + drawnow; + hold off +end % animation +hold off + +%% check trajectories +for z = 1:length(robot) + figure(); hold on; axis equal; + plot(robot{z}.q(:,1),robot{z}.q(:,2),'b') + plot(pf{z}.xEst(:,1),pf{z}.xEst(:,2),'r') + hold off +end diff --git a/icp/demo.m b/icp/demo.m deleted file mode 100644 index 9f5c7ef..0000000 --- a/icp/demo.m +++ /dev/null @@ -1,160 +0,0 @@ -%% demo.m -% -% Shows a couple of sample registrations using -% ICP - Iterative Closest Point -% -% Jakob Wilm and Martin Kjer, Technical University of Denmark, 2012 -clear all -clc -close all - -m = 80; % width of grid -n = m^2; % number of points - -[X,Y] = meshgrid(linspace(-2,2,m), linspace(-2,2,m)); - -X = reshape(X,1,[]); -Y = reshape(Y,1,[]); - -Z = sin(X).*cos(Y); - -% Create the data point-matrix -D = [X; Y; Z]; - -% Translation values (a.u.): -Tx = 0.5; -Ty = -0.3; -Tz = 0.2; - -% Translation vector -T = [Tx; Ty; Tz]; - -% Rotation values (rad.): -rx = 0.3; -ry = -0.2; -rz = 0.05; - -Rx = [1 0 0; - 0 cos(rx) -sin(rx); - 0 sin(rx) cos(rx)]; - -Ry = [cos(ry) 0 sin(ry); - 0 1 0; - -sin(ry) 0 cos(ry)]; - -Rz = [cos(rz) -sin(rz) 0; - sin(rz) cos(rz) 0; - 0 0 1]; - -% Rotation matrix -R = Rx*Ry*Rz; - -% Transform data-matrix plus noise into model-matrix -M = R * D + repmat(T, 1, n); - -% Add noise to model and data -rng(2912673); -M = M + 0.01*randn(3,n); -D = D + 0.01*randn(3,n); - -%% Run ICP (standard settings) -[Ricp Ticp ER t] = icp(M, D, 15); - -% Transform data-matrix using ICP result -Dicp = Ricp * D + repmat(Ticp, 1, n); - -% Plot model points blue and transformed points red -figure; -subplot(2,2,1); -plot3(M(1,:),M(2,:),M(3,:),'bo',D(1,:),D(2,:),D(3,:),'r.'); -axis equal; -xlabel('x'); ylabel('y'); zlabel('z'); -title('Red: z=sin(x)*cos(y), blue: transformed point cloud'); - -% Plot the results -subplot(2,2,2); -plot3(M(1,:),M(2,:),M(3,:),'bo',Dicp(1,:),Dicp(2,:),Dicp(3,:),'r.'); -axis equal; -xlabel('x'); ylabel('y'); zlabel('z'); -title('ICP result'); - -% Plot RMS curve -subplot(2,2,[3 4]); -plot(0:15,ER,'--x'); -xlabel('iteration#'); -ylabel('d_{RMS}'); -legend('bruteForce matching'); -title(['Total elapsed time: ' num2str(t(end),2) ' s']); - -%% Run ICP (fast kDtree matching and extrapolation) -[Ricp Ticp ER t] = icp(M, D, 15, 'Matching', 'kDtree', 'Extrapolation', true); - -% Transform data-matrix using ICP result -Dicp = Ricp * D + repmat(Ticp, 1, n); - -% Plot model points blue and transformed points red -figure; -subplot(2,2,1); -plot3(M(1,:),M(2,:),M(3,:),'bo',D(1,:),D(2,:),D(3,:),'r.'); -axis equal; -xlabel('x'); ylabel('y'); zlabel('z'); -title('Red: z=sin(x)*cos(y), blue: transformed point cloud'); - -% Plot the results -subplot(2,2,2); -plot3(M(1,:),M(2,:),M(3,:),'bo',Dicp(1,:),Dicp(2,:),Dicp(3,:),'r.'); -axis equal; -xlabel('x'); ylabel('y'); zlabel('z'); -title('ICP result'); - -% Plot RMS curve -subplot(2,2,[3 4]); -plot(0:15,ER,'--x'); -xlabel('iteration#'); -ylabel('d_{RMS}'); -legend('kDtree matching and extrapolation'); -title(['Total elapsed time: ' num2str(t(end),2) ' s']); - -%% Run ICP (partial data) - -% Partial model point cloud -Mp = M(:,Y>=0); - -% Boundary of partial model point cloud -b = (abs(X(Y>=0)) == 2) | (Y(Y>=0) == min(Y(Y>=0))) | (Y(Y>=0) == max(Y(Y>=0))); -bound = find(b); - -% Partial data point cloud -Dp = D(:,X>=0); - -[Ricp Ticp ER t] = icp(Mp, Dp, 50, 'EdgeRejection', true, 'Boundary', bound, 'Matching', 'kDtree'); - -% Transform data-matrix using ICP result -Dicp = Ricp * Dp + repmat(Ticp, 1, size(Dp,2)); - -% Plot model points blue and transformed points red -figure; -subplot(2,2,1); -plot3(Mp(1,not(b)),Mp(2,not(b)),Mp(3,not(b)),'bo',... - Mp(1,b),Mp(2,b),Mp(3,b),'go',... - Dp(1,:),Dp(2,:),Dp(3,:),'r.') -axis equal; -xlabel('x'); ylabel('y'); zlabel('z'); -title('Red: z=sin(x)*cos(y), blue: transformed point cloud'); - -% Plot the results -subplot(2,2,2); -plot3(Mp(1,not(b)),Mp(2,not(b)),Mp(3,not(b)),'bo',... - Mp(1,b),Mp(2,b),Mp(3,b),'go',... - Dicp(1,:),Dicp(2,:),Dicp(3,:),'r.'); -axis equal; -xlabel('x'); ylabel('y'); zlabel('z'); -title('ICP result'); - -% Plot RMS curve -subplot(2,2,[3 4]); -plot(0:50,ER,'--x'); -xlabel('iteration#'); -ylabel('d_{RMS}'); -legend('partial overlap'); -title(['Total elapsed time: ' num2str(t(end),2) ' s']); \ No newline at end of file diff --git a/icp/icp.m b/icp/icp.m deleted file mode 100644 index 1a6fa75..0000000 --- a/icp/icp.m +++ /dev/null @@ -1,592 +0,0 @@ -function [TR, TT, ER, t] = icp(q,p,varargin) -% Perform the Iterative Closest Point algorithm on three dimensional point -% clouds. -% -% [TR, TT] = icp(q,p) returns the rotation matrix TR and translation -% vector TT that minimizes the distances from (TR * p + TT) to q. -% p is a 3xm matrix and q is a 3xn matrix. -% -% [TR, TT] = icp(q,p,k) forces the algorithm to make k iterations -% exactly. The default is 10 iterations. -% -% [TR, TT, ER] = icp(q,p,k) also returns the RMS of errors for k -% iterations in a (k+1)x1 vector. ER(0) is the initial error. -% -% [TR, TT, ER, t] = icp(q,p,k) also returns the calculation times per -% iteration in a (k+1)x1 vector. t(0) is the time consumed for preprocessing. -% -% Additional settings may be provided in a parameter list: -% -% Boundary -% {[]} | 1x? vector -% If EdgeRejection is set, a vector can be provided that indexes into -% q and specifies which points of q are on the boundary. -% -% EdgeRejection -% {false} | true -% If EdgeRejection is true, point matches to edge vertices of q are -% ignored. Requires that boundary points of q are specified using -% Boundary or that a triangulation matrix for q is provided. -% -% Extrapolation -% {false} | true -% If Extrapolation is true, the iteration direction will be evaluated -% and extrapolated if possible using the method outlined by -% Besl and McKay 1992. -% -% Matching -% {bruteForce} | Delaunay | kDtree -% Specifies how point matching should be done. -% bruteForce is usually the slowest and kDtree is the fastest. -% Note that the kDtree option is depends on the Statistics Toolbox -% v. 7.3 or higher. -% -% Minimize -% {point} | plane | lmaPoint -% Defines whether point to point or point to plane minimization -% should be performed. point is based on the SVD approach and is -% usually the fastest. plane will often yield higher accuracy. It -% uses linearized angles and requires surface normals for all points -% in q. Calculation of surface normals requires substantial pre -% proccessing. -% The option lmaPoint does point to point minimization using the non -% linear least squares Levenberg Marquardt algorithm. Results are -% generally the same as in points, but computation time may differ. -% -% Normals -% {[]} | n x 3 matrix -% A matrix of normals for the n points in q might be provided. -% Normals of q are used for point to plane minimization. -% Else normals will be found through a PCA of the 4 nearest -% neighbors. -% -% ReturnAll -% {false} | true -% Determines whether R and T should be returned for all iterations -% or only for the last one. If this option is set to true, R will be -% a 3x3x(k+1) matrix and T will be a 3x1x(k+1) matrix. -% -% Triangulation -% {[]} | ? x 3 matrix -% A triangulation matrix for the points in q can be provided, -% enabling EdgeRejection. The elements should index into q, defining -% point triples that act together as triangles. -% -% Verbose -% {false} | true -% Enables extrapolation output in the Command Window. -% -% Weight -% {@(match)ones(1,m)} | Function handle -% For point or plane minimization, a function handle to a weighting -% function can be provided. The weighting function will be called -% with one argument, a 1xm vector that specifies point pairs by -% indexing into q. The weighting function should return a 1xm vector -% of weights for every point pair. -% -% WorstRejection -% {0} | scalar in ]0; 1[ -% Reject a given percentage of the worst point pairs, based on their -% Euclidean distance. -% -% Martin Kjer and Jakob Wilm, Technical University of Denmark, 2012 - -% Use the inputParser class to validate input arguments. -inp = inputParser; - -inp.addRequired('q', @(x)isreal(x) && size(x,1) == 3); -inp.addRequired('p', @(x)isreal(x) && size(x,1) == 3); - -inp.addOptional('iter', 10, @(x)x > 0 && x < 10^5); - -inp.addParamValue('Boundary', [], @(x)size(x,1) == 1); - -inp.addParamValue('EdgeRejection', false, @(x)islogical(x)); - -inp.addParamValue('Extrapolation', false, @(x)islogical(x)); - -validMatching = {'bruteForce','Delaunay','kDtree'}; -inp.addParamValue('Matching', 'bruteForce', @(x)any(strcmpi(x,validMatching))); - -validMinimize = {'point','plane','lmapoint'}; -inp.addParamValue('Minimize', 'point', @(x)any(strcmpi(x,validMinimize))); - -inp.addParamValue('Normals', [], @(x)isreal(x) && size(x,1) == 3); - -inp.addParamValue('NormalsData', [], @(x)isreal(x) && size(x,1) == 3); - -inp.addParamValue('ReturnAll', false, @(x)islogical(x)); - -inp.addParamValue('Triangulation', [], @(x)isreal(x) && size(x,2) == 3); - -inp.addParamValue('Verbose', false, @(x)islogical(x)); - -inp.addParamValue('Weight', @(x)ones(1,length(x)), @(x)isa(x,'function_handle')); - -inp.addParamValue('WorstRejection', 0, @(x)isscalar(x) && x > 0 && x < 1); - -inp.parse(q,p,varargin{:}); -arg = inp.Results; -clear('inp'); - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% Actual implementation - -% Allocate vector for RMS of errors in every iteration. -t = zeros(arg.iter+1,1); - -% Start timer -tic; - -Np = size(p,2); - -% Transformed data point cloud -pt = p; - -% Allocate vector for RMS of errors in every iteration. -ER = zeros(arg.iter+1,1); - -% Initialize temporary transform vector and matrix. -T = zeros(3,1); -R = eye(3,3); - -% Initialize total transform vector(s) and rotation matric(es). -TT = zeros(3,1, arg.iter+1); -TR = repmat(eye(3,3), [1,1, arg.iter+1]); - -% If Minimize == 'plane', normals are needed -if (strcmp(arg.Minimize, 'plane') && isempty(arg.Normals)) - arg.Normals = lsqnormest(q,4); -end - -% If Matching == 'Delaunay', a triangulation is needed -if strcmp(arg.Matching, 'Delaunay') - DT = DelaunayTri(transpose(q)); -end - -% If Matching == 'kDtree', a kD tree should be built (req. Stat. TB >= 7.3) -if strcmp(arg.Matching, 'kDtree') - kdOBJ = KDTreeSearcher(transpose(q)); -end - -% If edge vertices should be rejected, find edge vertices -if arg.EdgeRejection - if isempty(arg.Boundary) - bdr = find_bound(q, arg.Triangulation); - else - bdr = arg.Boundary; - end -end - -if arg.Extrapolation - % Initialize total transform vector (quaternion ; translation vec.) - qq = [ones(1,arg.iter+1);zeros(6,arg.iter+1)]; - % Allocate vector for direction change and change angle. - dq = zeros(7,arg.iter+1); - theta = zeros(1,arg.iter+1); -end - -t(1) = toc; - -% Go into main iteration loop -for k=1:arg.iter - - % Do matching - switch arg.Matching - case 'bruteForce' - [match mindist] = match_bruteForce(q,pt); - case 'Delaunay' - [match mindist] = match_Delaunay(q,pt,DT); - case 'kDtree' - [match mindist] = match_kDtree(q,pt,kdOBJ); - end - - % If matches to edge vertices should be rejected - if arg.EdgeRejection - p_idx = not(ismember(match, bdr)); - q_idx = match(p_idx); - mindist = mindist(p_idx); - else - p_idx = true(1, Np); - q_idx = match; - end - - % If worst matches should be rejected - if arg.WorstRejection - edge = round((1-arg.WorstRejection)*sum(p_idx)); - pairs = find(p_idx); - [~, idx] = sort(mindist); - p_idx(pairs(idx(edge:end))) = false; - q_idx = match(p_idx); - mindist = mindist(p_idx); - end - - if k == 1 - ER(k) = sqrt(sum(mindist.^2)/length(mindist)); - end - - switch arg.Minimize - case 'point' - % Determine weight vector - weights = arg.Weight(match); - [R,T] = eq_point(q(:,q_idx),pt(:,p_idx), weights(p_idx)); - case 'plane' - weights = arg.Weight(match); - [R,T] = eq_plane(q(:,q_idx),pt(:,p_idx),arg.Normals(:,q_idx),weights(p_idx)); - case 'lmaPoint' - [R,T] = eq_lmaPoint(q(:,q_idx),pt(:,p_idx)); - end - - % Add to the total transformation - TR(:,:,k+1) = R*TR(:,:,k); - TT(:,:,k+1) = R*TT(:,:,k)+T; - - % Apply last transformation - pt = TR(:,:,k+1) * p + repmat(TT(:,:,k+1), 1, Np); - - % Root mean of objective function - ER(k+1) = rms_error(q(:,q_idx), pt(:,p_idx)); - - % If Extrapolation, we might be able to move quicker - if arg.Extrapolation - qq(:,k+1) = [rmat2quat(TR(:,:,k+1));TT(:,:,k+1)]; - dq(:,k+1) = qq(:,k+1) - qq(:,k); - theta(k+1) = (180/pi)*acos(dot(dq(:,k),dq(:,k+1))/(norm(dq(:,k))*norm(dq(:,k+1)))); - if arg.Verbose - disp(['Direction change ' num2str(theta(k+1)) ' degree in iteration ' num2str(k)]); - end - if k>2 && theta(k+1) < 10 && theta(k) < 10 - d = [ER(k+1), ER(k), ER(k-1)]; - v = [0, -norm(dq(:,k+1)), -norm(dq(:,k))-norm(dq(:,k+1))]; - vmax = 25 * norm(dq(:,k+1)); - dv = extrapolate(v,d,vmax); - if dv ~= 0 - q_mark = qq(:,k+1) + dv * dq(:,k+1)/norm(dq(:,k+1)); - q_mark(1:4) = q_mark(1:4)/norm(q_mark(1:4)); - qq(:,k+1) = q_mark; - TR(:,:,k+1) = quat2rmat(qq(1:4,k+1)); - TT(:,:,k+1) = qq(5:7,k+1); - % Reapply total transformation - pt = TR(:,:,k+1) * p + repmat(TT(:,:,k+1), 1, Np); - % Recalculate root mean of objective function - % Note this is costly and only for fun! - switch arg.Matching - case 'bruteForce' - [~, mindist] = match_bruteForce(q,pt); - case 'Delaunay' - [~, mindist] = match_Delaunay(q,pt,DT); - case 'kDtree' - [~, mindist] = match_kDtree(q,pt,kdOBJ); - end - ER(k+1) = sqrt(sum(mindist.^2)/length(mindist)); - end - end - end - t(k+1) = toc; -end - -if not(arg.ReturnAll) - TR = TR(:,:,end); - TT = TT(:,:,end); -end - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -function [match mindist] = match_bruteForce(q, p) - m = size(p,2); - n = size(q,2); - match = zeros(1,m); - mindist = zeros(1,m); - for ki=1:m - d=zeros(1,n); - for ti=1:3 - d=d+(q(ti,:)-p(ti,ki)).^2; - end - [mindist(ki),match(ki)]=min(d); - end - - mindist = sqrt(mindist); - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -function [match mindist] = match_Delaunay(q, p, DT) - match = transpose(nearestNeighbor(DT, transpose(p))); - mindist = sqrt(sum((p-q(:,match)).^2,1)); - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -function [match mindist] = match_kDtree(~, p, kdOBJ) - [match mindist] = knnsearch(kdOBJ,transpose(p)); - match = transpose(match); - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -function [R,T] = eq_point(q,p,weights) - -m = size(p,2); -n = size(q,2); - -% normalize weights -weights = weights ./ sum(weights); - -% find data centroid and deviations from centroid -q_bar = q * transpose(weights); -q_mark = q - repmat(q_bar, 1, n); -% Apply weights -q_mark = q_mark .* repmat(weights, 3, 1); - -% find data centroid and deviations from centroid -p_bar = p * transpose(weights); -p_mark = p - repmat(p_bar, 1, m); -% Apply weights -%p_mark = p_mark .* repmat(weights, 3, 1); - -N = p_mark*transpose(q_mark); % taking points of q in matched order - -[U,~,V] = svd(N); % singular value decomposition - -R = V*diag([1 1 det(U*V')])*transpose(U); - -T = q_bar - R*p_bar; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -function [R,T] = eq_plane(q,p,n,weights) - -n = n .* repmat(weights,3,1); - -c = cross(p,n); - -cn = vertcat(c,n); - -C = cn*transpose(cn); - -b = - [sum(sum((p-q).*repmat(cn(1,:),3,1).*n)); - sum(sum((p-q).*repmat(cn(2,:),3,1).*n)); - sum(sum((p-q).*repmat(cn(3,:),3,1).*n)); - sum(sum((p-q).*repmat(cn(4,:),3,1).*n)); - sum(sum((p-q).*repmat(cn(5,:),3,1).*n)); - sum(sum((p-q).*repmat(cn(6,:),3,1).*n))]; - -X = C\b; - -cx = cos(X(1)); cy = cos(X(2)); cz = cos(X(3)); -sx = sin(X(1)); sy = sin(X(2)); sz = sin(X(3)); - -R = [cy*cz cz*sx*sy-cx*sz cx*cz*sy+sx*sz; - cy*sz cx*cz+sx*sy*sz cx*sy*sz-cz*sx; - -sy cy*sx cx*cy]; - -T = X(4:6); - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -function [R,T] = eq_lmaPoint(q,p) - -Rx = @(a)[1 0 0; - 0 cos(a) -sin(a); - 0 sin(a) cos(a)]; - -Ry = @(b)[cos(b) 0 sin(b); - 0 1 0; - -sin(b) 0 cos(b)]; - -Rz = @(g)[cos(g) -sin(g) 0; - sin(g) cos(g) 0; - 0 0 1]; - -Rot = @(x)Rx(x(1))*Ry(x(2))*Rz(x(3)); - -myfun = @(x,xdata)Rot(x(1:3))*xdata+repmat(x(4:6),1,length(xdata)); - - -options = optimset('Algorithm', 'levenberg-marquardt'); -x = lsqcurvefit(myfun, zeros(6,1), p, q, [], [], options); - - -R = Rot(x(1:3)); -T = x(4:6); - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% Extrapolation in quaternion space. Details are found in: -% -% Besl, P., & McKay, N. (1992). A method for registration of 3-D shapes. -% IEEE Transactions on pattern analysis and machine intelligence, 239?256. - -function [dv] = extrapolate(v,d,vmax) - -p1 = polyfit(v,d,1); % linear fit -p2 = polyfit(v,d,2); % parabolic fit -v1 = -p1(2)/p1(1); % linear zero crossing -v2 = -p2(2)/(2*p2(1)); % polynomial top point - -if issorted([0 v2 v1 vmax]) || issorted([0 v2 vmax v1]) - disp('Parabolic update!'); - dv = v2; -elseif issorted([0 v1 v2 vmax]) || issorted([0 v1 vmax v2])... - || (v2 < 0 && issorted([0 v1 vmax])) - disp('Line based update!'); - dv = v1; -elseif v1 > vmax && v2 > vmax - disp('Maximum update!'); - dv = vmax; -else - disp('No extrapolation!'); - dv = 0; -end - - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% Determine the RMS error between two point equally sized point clouds with -% point correspondance. -% ER = rms_error(p1,p2) where p1 and p2 are 3xn matrices. - -function ER = rms_error(p1,p2) -dsq = sum(power(p1 - p2, 2),1); -ER = sqrt(mean(dsq)); - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% Converts (orthogonal) rotation matrices R to (unit) quaternion -% representations -% -% Input: A 3x3xn matrix of rotation matrices -% Output: A 4xn matrix of n corresponding quaternions -% -% http://en.wikipedia.org/wiki/Rotation_matrix#Quaternion - -function quaternion = rmat2quat(R) - -Qxx = R(1,1,:); -Qxy = R(1,2,:); -Qxz = R(1,3,:); -Qyx = R(2,1,:); -Qyy = R(2,2,:); -Qyz = R(2,3,:); -Qzx = R(3,1,:); -Qzy = R(3,2,:); -Qzz = R(3,3,:); - -w = 0.5 * sqrt(1+Qxx+Qyy+Qzz); -x = 0.5 * sign(Qzy-Qyz) .* sqrt(1+Qxx-Qyy-Qzz); -y = 0.5 * sign(Qxz-Qzx) .* sqrt(1-Qxx+Qyy-Qzz); -z = 0.5 * sign(Qyx-Qxy) .* sqrt(1-Qxx-Qyy+Qzz); - -quaternion = reshape([w;x;y;z],4,[]); - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% Converts (unit) quaternion representations to (orthogonal) rotation matrices R -% -% Input: A 4xn matrix of n quaternions -% Output: A 3x3xn matrix of corresponding rotation matrices -% -% http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#From_a_quaternion_to_an_orthogonal_matrix - -function R = quat2rmat(quaternion) -q0(1,1,:) = quaternion(1,:); -qx(1,1,:) = quaternion(2,:); -qy(1,1,:) = quaternion(3,:); -qz(1,1,:) = quaternion(4,:); - -R = [q0.^2+qx.^2-qy.^2-qz.^2 2*qx.*qy-2*q0.*qz 2*qx.*qz+2*q0.*qy; - 2*qx.*qy+2*q0.*qz q0.^2-qx.^2+qy.^2-qz.^2 2*qy.*qz-2*q0.*qx; - 2*qx.*qz-2*q0.*qy 2*qy.*qz+2*q0.*qx q0.^2-qx.^2-qy.^2+qz.^2]; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% Least squares normal estimation from point clouds using PCA -% -% H. Hoppe, T. DeRose, T. Duchamp, J. McDonald, and W. Stuetzle. -% Surface reconstruction from unorganized points. -% In Proceedings of ACM Siggraph, pages 71:78, 1992. -% -% p should be a matrix containing the horizontally concatenated column -% vectors with points. k is a scalar indicating how many neighbors the -% normal estimation is based upon. -% -% Note that for large point sets, the function performs significantly -% faster if Statistics Toolbox >= v. 7.3 is installed. -% -% Jakob Wilm 2010 - -function n = lsqnormest(p, k) -m = size(p,2); -n = zeros(3,m); - -v = ver('stats'); -if str2double(v.Version) >= 7.5 - neighbors = transpose(knnsearch(transpose(p), transpose(p), 'k', k+1)); -else - neighbors = k_nearest_neighbors(p, p, k+1); -end - -for i = 1:m - x = p(:,neighbors(2:end, i)); - p_bar = 1/k * sum(x,2); - - P = (x - repmat(p_bar,1,k)) * transpose(x - repmat(p_bar,1,k)); %spd matrix P - %P = 2*cov(x); - - [V,D] = eig(P); - - [~, idx] = min(diag(D)); % choses the smallest eigenvalue - - n(:,i) = V(:,idx); % returns the corresponding eigenvector -end - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% Program to find the k - nearest neighbors (kNN) within a set of points. -% Distance metric used: Euclidean distance -% -% Note that this function makes repetitive use of min(), which seems to be -% more efficient than sort() for k < 30. - -function [neighborIds neighborDistances] = k_nearest_neighbors(dataMatrix, queryMatrix, k) - -numDataPoints = size(dataMatrix,2); -numQueryPoints = size(queryMatrix,2); - -neighborIds = zeros(k,numQueryPoints); -neighborDistances = zeros(k,numQueryPoints); - -D = size(dataMatrix, 1); %dimensionality of points - -for i=1:numQueryPoints - d=zeros(1,numDataPoints); - for t=1:D % this is to avoid slow repmat() - d=d+(dataMatrix(t,:)-queryMatrix(t,i)).^2; - end - for j=1:k - [s,t] = min(d); - neighborIds(j,i)=t; - neighborDistances(j,i)=sqrt(s); - d(t) = NaN; % remove found number from d - end -end - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% Boundary point determination. Given a set of 3D points and a -% corresponding triangle representation, returns those point indices that -% define the border/edge of the surface. - -function bound = find_bound(pts, poly) - -%Correcting polygon indices and converting datatype -poly = double(poly); -pts = double(pts); - -%Calculating freeboundary points: -TR = TriRep(poly, pts(1,:)', pts(2,:)', pts(3,:)'); -FF = freeBoundary(TR); - -%Output -bound = FF(:,1); - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - - \ No newline at end of file diff --git a/icp/icp_gmap (1).m b/icp/icp_gmap (1).m deleted file mode 100644 index 6eb4a2d..0000000 --- a/icp/icp_gmap (1).m +++ /dev/null @@ -1,24 +0,0 @@ -for i =1:1:326 - for j =1:1:326 - a(1,j+326*(i-1)) = i; - end -end - -for i =1:1:326 - for j =1:1:326 - a(2,j+326*(i-1)) = j; - end -end - -for i =1:1:326 - for j =1:1:326 - M(i,j); - end -end - -for i =1:1:326 - for j =1:1:326 - - a(2,j+326*(i-1)) = j; - end -end \ No newline at end of file diff --git a/icp/icp_gmap.m b/icp/icp_gmap.m deleted file mode 100644 index 6eb4a2d..0000000 --- a/icp/icp_gmap.m +++ /dev/null @@ -1,24 +0,0 @@ -for i =1:1:326 - for j =1:1:326 - a(1,j+326*(i-1)) = i; - end -end - -for i =1:1:326 - for j =1:1:326 - a(2,j+326*(i-1)) = j; - end -end - -for i =1:1:326 - for j =1:1:326 - M(i,j); - end -end - -for i =1:1:326 - for j =1:1:326 - - a(2,j+326*(i-1)) = j; - end -end \ No newline at end of file diff --git a/icp/license.txt b/icp/license.txt deleted file mode 100644 index 99b2429..0000000 --- a/icp/license.txt +++ /dev/null @@ -1,24 +0,0 @@ -Copyright (c) 2012, Jakob Wilm & Hans Martin Kjer -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are -met: - - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in - the documentation and/or other materials provided with the distribution - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -POSSIBILITY OF SUCH DAMAGE.