initial sd-vbs

initial sd-vbs

add sd-vbs

sd-vbs

1 files changed, 24 insertions, 0 deletions

diff --git a/SD-VBS/benchmarks/localization/src/matlab/quatRot.m b/SD-VBS/benchmarks/localization/src/matlab/quatRot.m
new file mode 100644
index 0000000..3688172
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/matlab/quatRot.m
@@ -0,0 +1,24 @@
+function ret=quatRot(vec, rQuat)
+nr=size(vec,1);
+tv = zeros(nr,1);
+vQuat=[tv, vec];
+temp = quatMul(rQuat, vQuat);
+temp1 = quatConj(rQuat);
+retVec = quatMul(temp, temp1);
+%retVec=quatMul(quatMul(rQuat, vQuat),quatConj(rQuat));
+%ret=retVec(:,2:4);
+rows = size(retVec, 1);
+ret = zeros(rows,3);
+
+for i=1:rows
+    k =1;
+    for j=2:4
+        ret(i,k) = retVec(i,j);
+        k = k+1;
+    end
+end
+%size(ret)
+
+
+
+