13 files changed, 1068 insertions, 0 deletions

diff --git a/SD-VBS/benchmarks/localization/src/c/eul2quat.c b/SD-VBS/benchmarks/localization/src/c/eul2quat.c
new file mode 100644
index 0000000..bea75cf
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/eul2quat.c
@@ -0,0 +1,50 @@
+/********************************
+Author: Sravanthi Kota Venkata
+********************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "localization.h"
+
+F2D* eul2quat(F2D* angle)
+{
+    F2D *ret;
+    F2D *x, *y, *z;
+    int k, i, j;
+    int rows, cols;
+
+    rows = angle->height;
+    cols = angle->width;
+
+    x = fDeepCopyRange(angle, 0, angle->height, 0, 1);
+    y = fDeepCopyRange(angle, 0, angle->height, 1, 1);
+    z = fDeepCopyRange(angle, 0, angle->height, 2, 1);
+
+    ret = fSetArray(x->height, 4, 0);
+    
+    for(i=0; i<rows; i++)
+    {
+        float xi, yi, zi;
+        k = 0;
+        xi = asubsref(x,i);
+        yi = asubsref(y,i);
+        zi = asubsref(z,i);
+
+        subsref(ret,i,k) = cos(xi/2)*cos(yi/2)*cos(zi/2)+sin(xi/2)*sin(yi/2)*sin(zi/2);
+        k++;
+        subsref(ret,i,k) = sin(xi/2)*cos(yi/2)*cos(zi/2)-cos(xi/2)*sin(yi/2)*sin(zi/2);
+        k++;
+        subsref(ret,i,k) = cos(xi/2)*sin(yi/2)*cos(zi/2)+sin(xi/2)*cos(yi/2)*sin(zi/2);
+        k++;
+        subsref(ret,i,k) = cos(xi/2)*cos(yi/2)*sin(zi/2)-sin(xi/2)*sin(yi/2)*cos(zi/2);
+    }
+
+    fFreeHandle(x);
+    fFreeHandle(y);
+    fFreeHandle(z);
+
+    return ret;
+}
+
+
+
diff --git a/SD-VBS/benchmarks/localization/src/c/generateSample.c b/SD-VBS/benchmarks/localization/src/c/generateSample.c
new file mode 100644
index 0000000..3acb4d7
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/generateSample.c
@@ -0,0 +1,88 @@
+/********************************
+Author: Sravanthi Kota Venkata
+********************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "localization.h"
+
+void generateSample(F2D *w, F2D *quat, F2D *vel, F2D *pos)
+{
+    int rows, cols, i, j, index;
+    I2D *sampleXId;
+    F2D *retQuat, *retVel, *retPos;
+
+    sampleXId = weightedSample(w);
+
+    rows = sampleXId->height;
+    cols = sampleXId->width;
+
+    if(cols > 1)
+        printf("ERROR: Cols more than 1.. Handle this case \n");
+
+    retQuat = fSetArray(quat->height, quat->width, 0);
+    retVel = fSetArray(vel->height, vel->width, 0);
+    retPos = fSetArray(pos->height, pos->width, 0);
+
+    for(i=0; i<rows; i++)
+    {
+        index = asubsref(sampleXId, i) - 1;
+        for(j=0; j<quat->width; j++)
+        {
+            subsref(retQuat,i,j) = subsref(quat,index,j);
+        }
+    }
+
+    for(i=0; i<rows; i++)
+    {
+        index = asubsref(sampleXId, i) - 1;
+        for(j=0; j<vel->width; j++)
+        {
+            subsref(retVel,i,j) = subsref(vel,index,j);
+        }
+    }
+    
+    for(i=0; i<rows; i++)
+    {
+        index = asubsref(sampleXId, i) - 1;
+        for(j=0; j<pos->width; j++)
+        {
+            subsref(retPos,i,j) = subsref(pos,index,j);
+        }
+    }
+
+    for(i=0; i<quat->height; i++)
+    {
+        for(j=0; j<quat->width; j++)
+        {
+            subsref(quat,i,j) = subsref(retQuat,i,j);
+        }
+    }
+
+    for(i=0; i<vel->height; i++)
+    {
+        for(j=0; j<vel->width; j++)
+        {
+            subsref(vel,i,j) = subsref(retVel,i,j);
+        }
+    }
+    
+    for(i=0; i<pos->height; i++)
+    {
+        for(j=0; j<pos->width; j++)
+        {
+            subsref(pos,i,j) = subsref(retPos,i,j);
+        }
+    }
+    
+    fFreeHandle(retQuat);
+    fFreeHandle(retVel);
+    fFreeHandle(retPos);
+    iFreeHandle(sampleXId);
+    
+    return;
+}
+
+
+
+
diff --git a/SD-VBS/benchmarks/localization/src/c/get3DGaussianProb.c b/SD-VBS/benchmarks/localization/src/c/get3DGaussianProb.c
new file mode 100644
index 0000000..58c4475
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/get3DGaussianProb.c
@@ -0,0 +1,49 @@
+/********************************
+Author: Sravanthi Kota Venkata
+********************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "localization.h"
+
+F2D* get3DGaussianProb( F2D* data, F2D* mean, F2D* A)
+{
+    F2D *p, *diff, *temp1, *temp2, *mt;
+    float temp;
+    int n_data, n_channel;
+    int i, j, k;
+    F2D* t;
+    float pi = 3.1412;
+
+    n_data = data->height;
+    n_channel = data->width;
+
+    t = fSetArray(n_data, 1, 1);
+
+    mt = fMtimes(t, mean); 
+    diff = fMinus( data, mt);
+    p = fSetArray(diff->height, 1, 0);
+
+    temp = sqrt(1.0/(pow(2*pi, n_channel)));
+    temp2 = randWrapper(diff->height,1);
+
+    j = (temp2->height*temp2->width);
+    for(i=0; i<j; i++)
+    {
+        float temp2i;
+        temp2i = asubsref(temp2,i);