initial sd-vbs

initial sd-vbs

add sd-vbs

sd-vbs

1 files changed, 25 insertions, 0 deletions

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;
+
+