20 files changed, 0 insertions, 611 deletions

diff --git a/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m b/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m
deleted file mode 100644
index 3d2ac76..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m
+++ /dev/null
@@ -1,12 +0,0 @@
-function retQuat=addEulNoise(quat, STD)
-n=size(quat,1);
-
-% KVS: replacing randnWrapper(n,3) with rand(n,3)
-gyro=randnWrapper(n,3)*STD;
-
-norm_gyro=sqrt(sum(gyro.^2,2));
-angleAlpha=norm_gyro;
-quatDelta=[cos(angleAlpha/2), gyro./(norm_gyro*ones(1,3)).*(sin(angleAlpha/2)*ones(1,3))];
-
-retQuat=quatMul(quat, quatDelta);
-
diff --git a/SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m b/SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m
deleted file mode 100644
index 060713a..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m
+++ /dev/null
@@ -1,11 +0,0 @@
-function [meanColor A]=calculate3DGaussian(data)
-n_data=size(data,1);
-n_channel=size(data,2);
-meanColor=mean(data);
-diff=double(data)-ones(n_data,1)*meanColor;
-diifTr = transpose(diff);
-Ainv=(diffTr*diff/n_data);
-AinvTr = transpose(Ainv);
-A=inv(AinvTr);
-
-
diff --git a/SD-VBS/benchmarks/localization/src/matlab/drawLog.m b/SD-VBS/benchmarks/localization/src/matlab/drawLog.m
deleted file mode 100644
index f88e078..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/drawLog.m
+++ /dev/null
@@ -1,44 +0,0 @@
-function drawLog(data, ground_data)
-subplot(1,3,1);
-[nr nc]=size(data);
-for i=1:nr
-    x=data(i,2);
-    y=data(i,3);
-    z=data(i,4);
-    R=[1 0 0; 0 cos(x) sin(x); 0 -sin(x) cos(x)]...
-        *[cos(y) 0 -sin(y); 0 1 0; sin(y) 0 cos(y)]...
-        *[cos(z) sin(z) 0; -sin(z) cos(z) 0; 0 0 1];
-    coord=R*[1 0 0]';
-    axis([-1 1 -1 1 -1 1]);axis on
-    plot3([0 coord(1)], [0 coord(2)], [0 coord(3)], 'b'); hold on
-end
-
-[nr nc]=size(ground_data);
-for i=1:nr
-x=ground_data(i,2);
-y=ground_data(i,3);
-z=ground_data(i,4);
-R=[1 0 0; 0 cos(x) sin(x); 0 -sin(x) cos(x)]...
-        *[cos(y) 0 -sin(y); 0 1 0; sin(y) 0 cos(y)]...
-        *[cos(z) sin(z) 0; -sin(z) cos(z) 0; 0 0 1];
-coord=R*[1 0 0]';
-coord2=R*[0 -1 0]';
-axis([-1 1 -1 1 -1 1]);axis on
-plot3([0 coord(1)], [0 coord(2)], [0 coord(3)], 'r'); hold on
-plot3([0 coord2(1)], [0 coord2(2)], [0 coord2(3)], 'g');
-end
-
-xlabel('x');
-ylabel('y');
-zlabel('z');
-hold off
-drawnow
-subplot(1,3,2);
-Xoffset=4422610;
-Yoffset=483660;
-axis([4422610-Xoffset 4422660-Xoffset 483620-Yoffset 483720-Yoffset]);axis on
-scatter(data(:,5)-Xoffset, data(:,6)-Yoffset, 8, 'b'); hold on
-axis([4422610-Xoffset 4422660-Xoffset 483620-Yoffset 483720-Yoffset]);axis on
-scatter(ground_data(:,5)-Xoffset, ground_data(:,6)-Yoffset, 10, 'r'); hold off
-drawnow
-
diff --git a/SD-VBS/benchmarks/localization/src/matlab/eul2quat.m b/SD-VBS/benchmarks/localization/src/matlab/eul2quat.m
deleted file mode 100644
index 57001a8..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/eul2quat.m
+++ /dev/null
@@ -1,9 +0,0 @@
-function retQuat=eul2quat(eulAngle)
-x=eulAngle(:,1);
-y=eulAngle(:,2);
-z=eulAngle(:,3);
-retQuat=[cos(x/2).*cos(y/2).*cos(z/2)+sin(x/2).*sin(y/2).*sin(z/2) ...
-        sin(x/2).*cos(y/2).*cos(z/2)-cos(x/2).*sin(y/2).*sin(z/2) ...
-        cos(x/2).*sin(y/2).*cos(z/2)+sin(x/2).*cos(y/2).*sin(z/2) ...
-        cos(x/2).*cos(y/2).*sin(z/2)-sin(x/2).*sin(y/2).*cos(z/2)];
-    
diff --git a/SD-VBS/benchmarks/localization/src/matlab/generateSample.m b/SD-VBS/benchmarks/localization/src/matlab/generateSample.m
deleted file mode 100644
index f3658f8..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/generateSample.m
+++ /dev/null
@@ -1,25 +0,0 @@
-function [retQuat retVel retPos]=generateSample(w, quat, vel, pos, STDDEV_VEL, STDDEV_POS)
-sampledXId=weightedSample(w);
-
-rows = size(sampledXId, 1);
-cols = size(sampledXId, 2);
-
-if(cols > 1)
-    disp(123456);
-end
-
-% retQuat = zeros(rows, 1);
-% retVel = zeros(rows, 1);
-% retPos = zeros(rows, 1);
-
-% for i=1:rows
-%     retQuat(i,1) = quat(sampleXId(i,1),1);
-%     retVel(i,1) = vel(sampleXId(i,1),1) + randnWrapper(1,1) * STDDEV_VEL;
-%     retPos(i,1) = pos(sampleXId(i,1),1) + randnWrapper(1,1) * STDDEV_POS;    
-% end
-
-retQuat=quat(sampledXId,:);
-retVel=vel(sampledXId,:);%+randnWrappern(n,3)*STDDEV_VEL;
-retPos=pos(sampledXId,:);%+randnWrappern(n,3)*STDDEV_POS;
-
-
diff --git a/SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m b/SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m
deleted file mode 100644
index 6a54530..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m
+++ /dev/null
@@ -1,22 +0,0 @@
-function p=get3DGaussianProb(data, mean, A)
-n_data=size(data,1);
-n_channel=size(data,2);
-
-p=zeros(n_data,1);
-diff=(data)-ones(n_data,1)*mean;
-detA = 1; %detA = det(A)
-dotA = randWrapper(size(diff,1),1); %dotA = dot(diff*A, diff, 2)
-p=sqrt(detA/((2*pi)^n_channel))*exp(-0.5*dotA);
-
-%% KVS If the above doesnt work, try uncommenting these lines below
-
-%%temp = (det(A)/((2*pi)^n_channel));
-%temp = (1.0/((2*pi)^n_channel));
-%temp1 = dot(diff*A,diff,2);
-%%temp1 = rand(1000,1); 
-%temp2 = exp(-0.5*temp1);
-%p = sqrt(temp) * exp(temp2);
-%
-
-
-
diff --git a/SD-VBS/benchmarks/localization/src/matlab/getGroundData.m b/SD-VBS/benchmarks/localization/src/matlab/getGroundData.m
deleted file mode 100644
index 7cf5f9c..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/getGroundData.m
+++ /dev/null
@@ -1,5 +0,0 @@
-function retData=getGroundData(data, tStamp)
-idx=find(data(:,1)==tStamp);
-retData=data(idx,:);
-
-
diff --git a/SD-VBS/benchmarks/localization/src/matlab/mcl.m b/SD-VBS/benchmarks/localization/src/matlab/mcl.m
deleted file mode 100644
index 8ab8aa6..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/mcl.m
+++ /dev/null
@@ -1,9 +0,0 @@
-function [retX, retW]=mcl(x,sData, invCov)
-%instead of using importance resampling, I assumed 3D gaussian for each
-%particle
-%retX=x+randn(nr,nc)*(invCov^-1); %add noise
-retX=x;
-retW=get3DGaussianProb(retX, sData, invCov);
-retW=retW/sum(retW);
-
-
diff --git a/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m b/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m
deleted file mode 100644
index 4e78b8f..0000000
--- a/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m
+++ /dev/null
@@ -1,18 +0,0 @@
-function retW=mclWhole(quat, Ovel, STDDEV_GPSVel, accl, STDDEV_ACCL)
-n=size(quat,1);
-OvelNorm=norm(Ovel);
-if (OvelNorm>0.5)
-    Ovel=Ovel/norm(Ovel);
-    %trick