initial sd-vbs

initial sd-vbs

add sd-vbs

sd-vbs

20 files changed, 611 insertions, 0 deletions

diff --git a/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m b/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m
new file mode 100644
index 0000000..3d2ac76
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m
@@ -0,0 +1,12 @@
+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
new file mode 100644
index 0000000..060713a
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m
@@ -0,0 +1,11 @@
+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
new file mode 100644
index 0000000..f88e078
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/drawLog.m
@@ -0,0 +1,44 @@
+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
new file mode 100644
index 0000000..57001a8
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/eul2quat.m
@@ -0,0 +1,9 @@
+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
new file mode 100644
index 0000000..f3658f8
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/generateSample.m
@@ -0,0 +1,25 @@
+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
new file mode 100644
index 0000000..6a54530
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m
@@ -0,0 +1,22 @@
+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
new file mode 100644
index 0000000..7cf5f9c
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/getGroundData.m
@@ -0,0 +1,5 @@
+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
new file mode 100644
index 0000000..8ab8aa6
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/mcl.m
@@ -0,0 +1,9 @@
+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
new file mode 100644
index 0000000..4e78b8f
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m
@@ -0,0 +1,18 @@