summaryrefslogtreecommitdiffstats
path: root/SD-VBS/benchmarks/localization/src
diff options
context:
space:
mode:
Diffstat (limited to 'SD-VBS/benchmarks/localization/src')
-rw-r--r--SD-VBS/benchmarks/localization/src/c/eul2quat.c50
-rw-r--r--SD-VBS/benchmarks/localization/src/c/generateSample.c88
-rw-r--r--SD-VBS/benchmarks/localization/src/c/get3DGaussianProb.c49
-rw-r--r--SD-VBS/benchmarks/localization/src/c/input.txt8
-rw-r--r--SD-VBS/benchmarks/localization/src/c/localization.h24
-rw-r--r--SD-VBS/benchmarks/localization/src/c/mcl.c34
-rw-r--r--SD-VBS/benchmarks/localization/src/c/quat2eul.c47
-rw-r--r--SD-VBS/benchmarks/localization/src/c/quatConj.c32
-rw-r--r--SD-VBS/benchmarks/localization/src/c/quatMul.c55
-rw-r--r--SD-VBS/benchmarks/localization/src/c/quatRot.c47
-rw-r--r--SD-VBS/benchmarks/localization/src/c/readSensorData.c63
-rw-r--r--SD-VBS/benchmarks/localization/src/c/script_localization.c534
-rw-r--r--SD-VBS/benchmarks/localization/src/c/weightedSample.c37
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 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include "localization.h"
8
9F2D* eul2quat(F2D* angle)
10{
11 F2D *ret;
12 F2D *x, *y, *z;
13 int k, i, j;
14 int rows, cols;
15
16 rows = angle->height;
17 cols = angle->width;
18
19 x = fDeepCopyRange(angle, 0, angle->height, 0, 1);
20 y = fDeepCopyRange(angle, 0, angle->height, 1, 1);
21 z = fDeepCopyRange(angle, 0, angle->height, 2, 1);
22
23 ret = fSetArray(x->height, 4, 0);
24
25 for(i=0; i<rows; i++)
26 {
27 float xi, yi, zi;
28 k = 0;
29 xi = asubsref(x,i);
30 yi = asubsref(y,i);
31 zi = asubsref(z,i);
32
33 subsref(ret,i,k) = cos(xi/2)*cos(yi/2)*cos(zi/2)+sin(xi/2)*sin(yi/2)*sin(zi/2);
34 k++;
35 subsref(ret,i,k) = sin(xi/2)*cos(yi/2)*cos(zi/2)-cos(xi/2)*sin(yi/2)*sin(zi/2);
36 k++;
37 subsref(ret,i,k) = cos(xi/2)*sin(yi/2)*cos(zi/2)+sin(xi/2)*cos(yi/2)*sin(zi/2);
38 k++;
39 subsref(ret,i,k) = cos(xi/2)*cos(yi/2)*sin(zi/2)-sin(xi/2)*sin(yi/2)*cos(zi/2);
40 }
41
42 fFreeHandle(x);
43 fFreeHandle(y);
44 fFreeHandle(z);
45
46 return ret;
47}
48
49
50
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 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include "localization.h"
8
9void generateSample(F2D *w, F2D *quat, F2D *vel, F2D *pos)
10{
11 int rows, cols, i, j, index;
12 I2D *sampleXId;
13 F2D *retQuat, *retVel, *retPos;
14
15 sampleXId = weightedSample(w);
16
17 rows = sampleXId->height;
18 cols = sampleXId->width;
19
20 if(cols > 1)
21 printf("ERROR: Cols more than 1.. Handle this case \n");
22
23 retQuat = fSetArray(quat->height, quat->width, 0);
24 retVel = fSetArray(vel->height, vel->width, 0);
25 retPos = fSetArray(pos->height, pos->width, 0);
26
27 for(i=0; i<rows; i++)
28 {
29 index = asubsref(sampleXId, i) - 1;
30 for(j=0; j<quat->width; j++)
31 {
32 subsref(retQuat,i,j) = subsref(quat,index,j);
33 }
34 }
35
36 for(i=0; i<rows; i++)
37 {
38 index = asubsref(sampleXId, i) - 1;
39 for(j=0; j<vel->width; j++)
40 {
41 subsref(retVel,i,j) = subsref(vel,index,j);
42 }
43 }
44
45 for(i=0; i<rows; i++)
46 {
47 index = asubsref(sampleXId, i) - 1;
48 for(j=0; j<pos->width; j++)
49 {
50 subsref(retPos,i,j) = subsref(pos,index,j);
51 }
52 }
53
54 for(i=0; i<quat->height; i++)
55 {
56 for(j=0; j<quat->width; j++)
57 {
58 subsref(quat,i,j) = subsref(retQuat,i,j);
59 }
60 }
61
62 for(i=0; i<vel->height; i++)
63 {
64 for(j=0; j<vel->width; j++)
65 {
66 subsref(vel,i,j) = subsref(retVel,i,j);
67 }
68 }
69
70 for(i=0; i<pos->height; i++)
71 {
72 for(j=0; j<pos->width; j++)
73 {
74 subsref(pos,i,j) = subsref(retPos,i,j);
75 }
76 }
77
78 fFreeHandle(retQuat);
79 fFreeHandle(retVel);
80 fFreeHandle(retPos);
81 iFreeHandle(sampleXId);
82
83 return;
84}
85
86
87
88
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 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include "localization.h"
8
9F2D* get3DGaussianProb( F2D* data, F2D* mean, F2D* A)
10{
11 F2D *p, *diff, *temp1, *temp2, *mt;
12 float temp;
13 int n_data, n_channel;
14 int i, j, k;
15 F2D* t;
16 float pi = 3.1412;
17
18 n_data = data->height;
19 n_channel = data->width;
20
21 t = fSetArray(n_data, 1, 1);
22
23 mt = fMtimes(t, mean);
24 diff = fMinus( data, mt);
25 p = fSetArray(diff->height, 1, 0);
26
27 temp = sqrt(1.0/(pow(2*pi, n_channel)));
28 temp2 = randWrapper(diff->height,1);
29
30 j = (temp2->height*temp2->width);
31 for(i=0; i<j; i++)
32 {
33 float temp2i;
34 temp2i = asubsref(temp2,i);
35
36 temp2i = exp(-0.5*temp2i);
37 asubsref(p,i) = temp2i*temp;
38 }
39
40 fFreeHandle(t);
41 fFreeHandle(temp2);
42 fFreeHandle(mt);
43 fFreeHandle(diff);
44
45 return p;
46}
47
48
49
diff --git a/SD-VBS/benchmarks/localization/src/c/input.txt b/SD-VBS/benchmarks/localization/src/c/input.txt
new file mode 100644
index 0000000..dd6d1cf
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/input.txt
@@ -0,0 +1,8 @@
15 6
211168.4 3 -0.098 0.247 9.59
311168.175 1 -0.000000 0.000000 0.000000
411168.04 4 4422.01 4836.36 33.99
5-0.0006 -0.0007 0.021 9.463 21.130
611168.4 3 -0.098 0.247 9.59
711168.4 3 -0.098 0.247 9.59
8
diff --git a/SD-VBS/benchmarks/localization/src/c/localization.h b/SD-VBS/benchmarks/localization/src/c/localization.h
new file mode 100644
index 0000000..b2c03bb
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/localization.h
@@ -0,0 +1,24 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#ifndef _LOCALIZATION_
6#define _LOCALIZATION_
7
8#include "sdvbs_common.h"
9
10int script_localization();
11F2D* eul2quat(F2D* angle);
12void generateSample(F2D *w, F2D *quat, F2D *vel, F2D *pos);
13F2D* get3DGaussianProb( F2D* data, F2D* mean, F2D* A);
14F2D* mcl(F2D* x, F2D* sData, F2D* invConv);
15F2D* quat2eul(F2D* quat);
16F2D* quatConj(F2D* a);
17F2D* quatMul(F2D* a, F2D* b);
18F2D* quatRot(F2D* vec, F2D* rQuat);
19F2D* readSensorData(I2D* index, F2D* fid, I2D* type, I2D* eof);
20I2D* weightedSample(F2D* w);
21
22#endif
23
24
diff --git a/SD-VBS/benchmarks/localization/src/c/mcl.c b/SD-VBS/benchmarks/localization/src/c/mcl.c
new file mode 100644
index 0000000..a3c56b6
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/mcl.c
@@ -0,0 +1,34 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include "localization.h"
8
9F2D* mcl(F2D* x, F2D* sData, F2D* invConv)
10{
11 int i, j;
12 F2D *retW, *retX, *sum;
13 float sumVal;
14
15 retX = fDeepCopy(x);
16 retW = get3DGaussianProb(retX, sData, invConv);
17 sum = fSum(retW);
18 if(sum->height == 1 && sum->width ==1)
19 {
20 sumVal = asubsref(sum,0);
21 for(i=0; i<retW->height; i++)
22 for(j=0; j<retW->width; j++)
23 subsref(retW,i,j) = subsref(retW,i,j)/sumVal;
24 }
25 else
26 retW = fMdivide(retW, sum);
27
28 fFreeHandle(retX);
29 fFreeHandle(sum);
30
31 return retW;
32}
33
34
diff --git a/SD-VBS/benchmarks/localization/src/c/quat2eul.c b/SD-VBS/benchmarks/localization/src/c/quat2eul.c
new file mode 100644
index 0000000..bc581c9
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/quat2eul.c
@@ -0,0 +1,47 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include "localization.h"
8#include <math.h>
9
10F2D* quat2eul(F2D* quat)
11{
12 F2D *retEul;
13 int i, j, k;
14 int rows, cols;
15
16 rows = quat->height;
17 cols = quat->width;
18
19 retEul = fSetArray(rows, 3, 0);
20
21 for(i=0; i<rows; i++)
22 {
23 float temp, temp1, temp2, temp3, temp4;
24 float quati2, quati3, quati1, quati0;
25
26 quati0 = subsref(quat,i,0);
27 quati1 = subsref(quat,i,1);
28 quati2 = subsref(quat,i,2);
29 quati3 = subsref(quat,i,3);
30
31 temp = 2 *quati2 * quati3 + quati0 * quati1;
32 temp1 = pow(quati0,2) - pow(quati1,2) - pow(quati2,2) + pow(quati3,2);
33 temp2 = -2*quati1 * quati2 + quati0 * quati3;
34 temp3 = 2*quati1 * quati2 + quati0 * quati3;
35 temp4 = pow(quati0,2) + pow(quati1,2) - pow(quati2,2) - pow(quati3,2);
36
37 asubsref(retEul,k++) = atan2(temp, temp1);
38 asubsref(retEul,k++) = asin(temp2);
39 asubsref(retEul,k++) = atan2(temp3, temp4);
40 }
41
42 return retEul;
43}
44
45
46
47
diff --git a/SD-VBS/benchmarks/localization/src/c/quatConj.c b/SD-VBS/benchmarks/localization/src/c/quatConj.c
new file mode 100644
index 0000000..6607186
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/quatConj.c
@@ -0,0 +1,32 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include "localization.h"
8
9F2D* quatConj(F2D* a)
10{
11 F2D* retQuat;
12 int rows, cols;
13 int i, j, k;
14
15 rows = a->height;
16 cols = a->width;
17 retQuat = fSetArray(rows, 4, 0);
18
19 for(i=0; i<rows; i++)
20 {
21 k=0;
22 subsref(retQuat,i,k++) = subsref(a,i,0);
23 subsref(retQuat,i,k++) = -subsref(a,i,1);
24 subsref(retQuat,i,k++) = -subsref(a,i,2);
25 subsref(retQuat,i,k) = -subsref(a,i,3);
26 }
27
28 return retQuat;
29}
30
31
32
diff --git a/SD-VBS/benchmarks/localization/src/c/quatMul.c b/SD-VBS/benchmarks/localization/src/c/quatMul.c
new file mode 100644
index 0000000..8b8dafe
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/quatMul.c
@@ -0,0 +1,55 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include "localization.h"
8
9F2D* quatMul(F2D* a, F2D* b)
10{
11 int ra, ca, rb, cb;
12 F2D *ret;
13 int i, j, k=0;
14
15 ra = a->height;
16 ca = a->width;
17
18 rb = b->height;
19 cb = b->width;
20
21 ret = fSetArray(ra, 4, 0);
22
23 j = 0;
24 for(i=0; i<ra; i++)
25 {
26 k = 0;
27 float ai0, ai1, ai2, ai3;
28 float bj0, bj1, bj2, bj3;
29
30 ai0 = subsref(a,i,0);
31 ai1 = subsref(a,i,1);
32 ai2 = subsref(a,i,2);
33 ai3 = subsref(a,i,3);
34
35 bj0 = subsref(b,j,0);
36 bj1 = subsref(b,j,1);
37 bj2 = subsref(b,j,2);
38 bj3 = subsref(b,j,3);
39
40 subsref(ret,i,k++) = ai0*bj0 - ai1*bj1 - ai2*bj2 - ai3*bj3;
41 subsref(ret,i,k++) = ai0*bj1 + ai1*bj0 + ai2*bj3 - ai3*bj2;
42 subsref(ret,i,k++) = ai0*bj2 - ai1*bj3 + ai2*bj0 + ai3*bj1;
43 subsref(ret,i,k++) = ai0*bj3 + ai1*bj2 - ai2*bj1 + ai3*bj0;
44
45 if(rb == ra)
46 j++;
47 }
48
49 return ret;
50}
51
52
53
54
55
diff --git a/SD-VBS/benchmarks/localization/src/c/quatRot.c b/SD-VBS/benchmarks/localization/src/c/quatRot.c
new file mode 100644
index 0000000..4962b55
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/quatRot.c
@@ -0,0 +1,47 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include "localization.h"
8
9F2D* quatRot(F2D* vec, F2D* rQuat)
10{
11 F2D *ret;
12 int nr, i, j, k, rows, cols;
13 F2D *tv, *vQuat, *temp, *temp1;
14 F2D *retVec;
15
16 nr = vec->height;
17 tv = fSetArray(nr, 1, 0);
18 vQuat = fHorzcat(tv, vec);
19 temp = quatMul(rQuat, vQuat);
20 temp1 = quatConj(rQuat);
21 retVec = quatMul(temp, temp1);
22
23 rows = retVec->height;
24 cols = retVec->width;
25
26 ret = fSetArray(rows, 3, 0);
27
28 for(i=0; i<rows; i++)
29 {
30 k = 0;
31 for(j=1; j<4; j++)
32 {
33 subsref(ret,i,k) = subsref(retVec,i,j);
34 k++;
35 }
36 }
37
38 fFreeHandle(tv);
39 fFreeHandle(vQuat);
40 fFreeHandle(temp);
41 fFreeHandle(temp1);
42 fFreeHandle(retVec);
43
44 return ret;
45}
46
47
diff --git a/SD-VBS/benchmarks/localization/src/c/readSensorData.c b/SD-VBS/benchmarks/localization/src/c/readSensorData.c
new file mode 100644
index 0000000..12c0dc0
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/readSensorData.c
@@ -0,0 +1,63 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include "localization.h"
8
9F2D* readSensorData(I2D* index, F2D* fid, I2D* type, I2D* eof)
10{
11 F2D *retData;
12 int rows, i, j, k;
13 int atype=-1, aindex;
14
15 aindex = asubsref(index, 0);
16
17 asubsref(index,0) = asubsref(index,0) + 1;
18 rows = fid->height;
19 asubsref(type,0) = 0;
20 retData = fSetArray(1, 8, 0);
21
22 if( asubsref(index,0) > (rows-1) )
23 asubsref(eof,0) = 1;
24 else
25 {
26 if( asubsref(index,0) == rows)
27 asubsref(eof,0) = 1;
28 else
29 asubsref(eof,0) = 0;
30
31 k = asubsref(index,0);
32 atype = subsref(fid, k, 1);
33 if( (atype == 1) || (atype == 2) || (atype == 3) )
34 {
35 for(i=0; i<3; i++)
36 {
37 asubsref(retData,i) = subsref(fid,k,(i+2));
38 }
39 }
40 if( atype == 4 )
41 {
42 for(i=0; i<3; i++)
43 {
44 asubsref(retData,i) = subsref(fid,k,(i+2));
45 }
46 for(i=3; i<8; i++)
47 {
48 asubsref(retData,i) = subsref(fid,k+1,(i-3));
49 }
50 aindex = aindex + 1;
51 }
52 aindex = aindex + 1;
53 }
54
55 asubsref(index,0) = aindex;
56 asubsref(type, 0) = atype;
57
58 return retData;
59}
60
61
62
63
diff --git a/SD-VBS/benchmarks/localization/src/c/script_localization.c b/SD-VBS/benchmarks/localization/src/c/script_localization.c
new file mode 100644
index 0000000..3392320
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/script_localization.c
@@ -0,0 +1,534 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include <malloc.h>
8#include "localization.h"
9#include "extra.h"
10#define LOCOLIZATION_MEM 1<<24
11int main(int argc, char* argv[])
12{
13 SET_UP
14 mallopt(M_TOP_PAD, LOCOLIZATION_MEM);
15 mallopt(M_MMAP_MAX, 0);
16 int n, i, j, k, icount=-1;
17 F2D* fid;
18 float gyroTimeInterval=0.01;
19 float acclTimeInterval=0.01;
20
21 float STDDEV_GPSVel=0.5;
22 float STDDEV_ODOVel=0.1;
23 float STDDEV_ACCL=1;
24 float M_STDDEV_GYRO=0.1;
25 float M_STDDEV_POS=0.1;
26 float M_STDDEV_VEL=0.02;
27
28 F2D *pos, *vel;
29 float pi = 3.1416;
30 F2D *eul1, *eul2, *quat;
31 F2D *sData, *gyro, *norm_gyro, *angleAlpha;
32 F2D *quatDelta, *Opos, *temp_STDDEV_GPSPos, *w;
33 F2D *qConj, *orgWorld, *accl, *gtemp;
34 F2D *gravity, *t1;
35 I2D *tStamp, *sType, *isEOF;
36 I2D *index;
37 int rows, cols;
38 F2D *resultMat;
39 F2D *STDDEV_GPSPos;
40 F2D *ones, *randW;
41
42 char im1[100];
43
44 printf("Input txt File: ");
45 scanf("%s", im1);
46
47 fid = readFile(im1);
48
49 n = 1000;
50
51 #ifdef test
52 n = 3;
53 gyroTimeInterval = 0.1;
54 acclTimeInterval = 0.1;
55 M_STDDEV_VEL = 0.2;
56 #endif
57 #ifdef sim_fast
58 n = 3;
59 #endif
60 #ifdef sim
61 n = 10;
62 #endif
63 #ifdef sqcif
64 n = 800;
65 #endif
66 #ifdef qcif
67 n = 500;
68 #endif
69 #ifdef vga
70 n = 2000;
71 #endif
72 #ifdef wuxga
73 n = 3000;
74 #endif
75
76 resultMat = fSetArray(3,fid->height, 0);
77
78 pos = fSetArray(n, 3, 0);
79 vel = fSetArray(n, 3, 0);
80 ones = fSetArray(n,1,1);
81
82
83 F2D *randn;
84
85 randn = randWrapper(n,3);
86 printf("start\n");
87 for_each_job{
88 fResetArray(pos,n, 3, 0);
89 fResetArray(vel,n, 3, 0);
90 fResetArray(ones,n,1,1);
91
92 {
93 int j;
94 for(i=0; i<n; i++)
95 for(j=0; j<3; j++)
96 subsref(vel, i, j) += subsref(randn,i,j) * STDDEV_ODOVel;
97
98 }
99
100
101
102 {
103 F2D *eulAngle, *randn;
104 eulAngle = fSetArray(n, 3, 0);
105 randn = randWrapper(n,1);
106
107 for(i=0; i<n; i++)
108 {
109 subsref(eulAngle, i, 2) = subsref(randn, i, 0) * 2 * pi;
110 }
111
112 eul1 = eul2quat(eulAngle);
113 fFreeHandle(eulAngle);
114
115 eulAngle = fSetArray(1, 3, 0);
116 subsref(eulAngle, 0, 0) = pi;
117 eul2 = eul2quat(eulAngle);
118
119 fFreeHandle(randn);
120 fFreeHandle(eulAngle);
121 }
122
123 quat = quatMul(eul1, eul2);
124 fFreeHandle(eul1);
125 fFreeHandle(eul2);
126
127 i=0;
128 index = iSetArray(1,1,-1);
129 sType = iSetArray(1,1,-1);
130 isEOF = iSetArray(1,1,-1);
131
132
133 rows =0;
134 cols = 5;
135 STDDEV_GPSPos = fSetArray(3,3,0);
136 randW = randnWrapper(n,3);
137 icount=-1;
138 while(1)
139 {
140 icount=icount+1;
141
142 /*
143 S = 4
144 A = 3
145 G = 2
146 V = 1
147 */
148
149 sData = readSensorData(index, fid, sType, isEOF);
150 rows++;
151
152
153
154 if( asubsref(sType,0) ==2)
155 {
156
157 //Motion model
158
159 {
160 int i;
161 F2D *t, *t1;
162 F2D *abc, *abcd;
163 int qD_r=0, qD_c=0;
164 F2D *cosA, *sinA;
165
166 t = fDeepCopyRange(sData, 0, 1, 0, 3);
167 gyro = fMtimes(ones, t);
168 abc = fMallocHandle(gyro->height, gyro->width);
169 t1 = fDeepCopy(randW);
170
171 for(i=0; i<(n*3); i++)
172 {
173 asubsref(t1,i) = asubsref(randW,i) * M_STDDEV_GYRO;
174 asubsref(gyro, i) += asubsref(t1,i);
175 asubsref(abc, i) = pow(asubsref(gyro, i), 2);
176 }
177 fFreeHandle(t1);
178 abcd = fSum2(abc, 2);
179
180 norm_gyro = fMallocHandle(abcd->height,abcd->width);
181 angleAlpha = fMallocHandle(abcd->height, abcd->width);
182
183 for(i=0; i<(abcd->height*abcd->width); i++)
184 {
185 asubsref(norm_gyro, i) = sqrt(asubsref(abcd,i));
186 asubsref(angleAlpha,i) = asubsref(norm_gyro,i) * gyroTimeInterval;
187 }
188
189 qD_r += angleAlpha->height + gyro->height;
190 qD_c += angleAlpha->width + 3;
191
192 fFreeHandle(t);
193 fFreeHandle(abcd);
194
195 cosA = fSetArray(angleAlpha->height, angleAlpha->width, 0);
196 sinA = fSetArray(angleAlpha->height, angleAlpha->width, 0);
197
198 for(i=0; i<(cosA->height*cosA->width); i++)
199 asubsref(cosA,i) = cos( asubsref(angleAlpha,i) /2 );
200
201 for(i=0; i<(sinA->height*sinA->width); i++)
202 asubsref(sinA,i) = sin( asubsref(angleAlpha,i) /2 );
203
204
205 fFreeHandle(abc);
206 abc = fSetArray(1,3,1);
207 t1 = fMtimes(norm_gyro, abc);
208 t = ffDivide(gyro, t1);
209 fFreeHandle(t1);
210
211 abcd = fMtimes(sinA, abc);
212 t1 = fTimes(t, abcd);
213 quatDelta = fHorzcat(cosA, t1);
214
215 fFreeHandle(abcd);
216 fFreeHandle(t);
217 fFreeHandle(t1);
218 fFreeHandle(abc);
219
220 t = quatMul(quat, quatDelta);
221 fFreeHandle(quat);
222 fFreeHandle(quatDelta);
223 quat = fDeepCopy(t);
224
225 fFreeHandle(t);
226 fFreeHandle(norm_gyro);
227 fFreeHandle(gyro);
228 fFreeHandle(angleAlpha);
229 fFreeHandle(cosA);
230 fFreeHandle(sinA);
231
232 }
233 }
234
235 if( asubsref(sType,0) ==4)
236 {
237 //Observation
238
239 float tempSum=0;
240 F2D *Ovel;
241 float OvelNorm;
242 int i;
243
244
245 asubsref(STDDEV_GPSPos, 0) = asubsref(sData, 6);
246 asubsref(STDDEV_GPSPos, 4) = asubsref(sData,7);
247 asubsref(STDDEV_GPSPos, 8) = 15;
248
249 Opos = fDeepCopyRange(sData, 0, 1, 0, 3);
250
251 //Initialize
252
253 for(i=0; i<(pos->height*pos->width); i++)
254 tempSum += asubsref(pos,i);
255
256 if(tempSum == 0)
257 {
258 F2D *t, *t1;
259 t = fMtimes( randW, STDDEV_GPSPos);
260 t1 = fMtimes(ones, Opos);
261
262 for(i=0; i<(pos->height*pos->width); i++)
263 asubsref(pos,i) = asubsref(t,i) + asubsref(t1,i);
264
265 fFreeHandle(t);
266 fFreeHandle(t1);
267 }
268 else
269 {
270 int rows, cols;
271 int mnrows, mncols;
272
273 rows = STDDEV_GPSPos->height;
274 cols = STDDEV_GPSPos->width;
275
276 temp_STDDEV_GPSPos = fSetArray(rows,cols,1);
277 for( mnrows=0; mnrows<rows; mnrows++)
278 {
279 for( mncols=0; mncols<cols; mncols++)
280 {
281 subsref(temp_STDDEV_GPSPos,mnrows,mncols) = pow(subsref(STDDEV_GPSPos,mnrows,mncols),-1);
282 }
283 }
284
285 w = mcl(pos, Opos , temp_STDDEV_GPSPos);
286 generateSample(w, quat, vel, pos);
287 }
288 fFreeHandle(Opos);
289
290 //compare direction
291 Ovel = fDeepCopyRange(sData, 0, 1, 3, 3);
292 OvelNorm=2;//1.1169e+09;
293
294 if (OvelNorm>0.5)
295 {
296 F2D *t;
297 t = fDeepCopy(Ovel);
298 fFreeHandle(Ovel);
299 /* This is a double precision division */
300 Ovel = fDivide(t, OvelNorm);
301 qConj = quatConj(quat);
302 fFreeHandle(t);
303
304 {
305 t = fSetArray(1,3,0);
306 subsref(t,0,0) = 1;
307 orgWorld = quatRot(t, qConj);
308 fFreeHandle(t);
309 fFreeHandle(qConj);
310 t = fSetArray(3,3,0);
311 asubsref(t,0) = 1;
312 asubsref(t,4) = 1;
313 asubsref(t,8) = 1;
314
315 {
316 int i;
317 for(i=0; i<(t->height*t->width); i++)
318 asubsref(t, i) = asubsref(t,i)/STDDEV_GPSVel;
319 w = mcl( orgWorld, Ovel, t);
320 generateSample(w, quat, vel, pos);
321 }
322
323 fFreeHandle(t);
324 fFreeHandle(w);
325 fFreeHandle(orgWorld);
326 }
327 }
328 fFreeHandle(Ovel);
329 }
330
331 if( asubsref(sType,0) ==1)
332 {
333 //Observation
334 F2D *Ovel;
335 F2D *t, *t1, *t2;
336 float valVel;
337
338 t = fSetArray(vel->height, 1, 0);
339
340 for(i=0; i<vel->height; i++)
341 {
342 subsref(t,i,0) = sqrt(pow(subsref(vel,i,0),2) + pow(subsref(vel,i,1),2) + pow(subsref(vel,i,2),2));
343 }
344
345 Ovel = fSetArray(1, 1, asubsref(sData,0));
346 valVel = 1.0/STDDEV_ODOVel;
347
348 t1 = fSetArray(1,1,(1.0/STDDEV_ODOVel));
349 w = mcl (t, Ovel, t1);
350 generateSample(w, quat, vel, pos);
351
352
353 fFreeHandle(w);
354 fFreeHandle(t);
355 fFreeHandle(t1);
356 fFreeHandle(Ovel);
357 }
358
359 if( asubsref(sType,0) ==3)
360 {
361 //Observation
362 F2D *t;
363 t = fSetArray(1, 3, 0);
364 asubsref(t,2) = -9.8;
365
366 accl = fDeepCopyRange(sData, 0, 1, 0, 3);
367 gtemp = fMtimes( ones, t);
368 gravity = quatRot(gtemp, quat);
369
370 fFreeHandle(gtemp);
371 fFreeHandle(t);
372 t = fSetArray(3,3,0);
373 asubsref(t,0) = 1;
374 asubsref(t,4) = 1;
375 asubsref(t,8) = 1;
376
377 {
378 int i;
379 for(i=0; i<(t->height*t->width); i++)
380 asubsref(t,i) = asubsref(t,i)/STDDEV_ACCL;
381 w = mcl( gravity, accl, t);
382 }
383
384 generateSample(w, quat, vel, pos);
385 fFreeHandle(t);
386 //Motion model
387 t = fMtimes(ones, accl);
388 fFreeHandle(accl);
389
390 accl = fMinus(t, gravity);
391
392 fFreeHandle(w);
393 fFreeHandle(gravity);
394 fFreeHandle(t);
395
396 {
397 //pos=pos+quatRot(vel,quatConj(quat))*acclTimeInterval+1/2*quatRot(accl,quatConj(quat))*acclTimeInterval^2+randn(n,3)*M_STDDEV_POS;
398
399 F2D *s, *is;
400 int i;
401 is = quatConj(quat);
402 s = quatRot(vel, is);
403 fFreeHandle(is);
404
405 for(i=0; i<(s->height*s->width); i++)
406 {
407 asubsref(s,i) = asubsref(s,i)*acclTimeInterval; //+(1/2);
408 }
409 is = fPlus(pos, s);
410 fFreeHandle(pos);
411 pos = fDeepCopy(is);
412 fFreeHandle(is);
413 fFreeHandle(s);
414
415 /** pos_ above stores: pos+quatRot(vel,quatConj(quat))*acclTimeInterval **/
416
417 is = quatConj(quat);
418 s = quatRot(accl, is);
419 t = fDeepCopy(s);
420 for(i=0; i<(s->height*s->width); i++)
421 {
422 asubsref(t,i) = 1/2*asubsref(s,i)*acclTimeInterval*acclTimeInterval;
423 }
424
425 /** t_ above stores: 1/2*quatRot(accl,quatCong(quat))*acclTimeInterval^2 **/
426
427 fFreeHandle(s);
428 fFreeHandle(is);
429 s = randnWrapper(n,3);
430
431 for(i=0; i<(s->height*s->width); i++)
432 {
433 asubsref(s,i) = asubsref(s,i) * M_STDDEV_POS;
434 }
435
436 /** s_ above stores: randn(n,3)*M_STDDEV_POS **/
437
438 is = fPlus(pos, t);
439 fFreeHandle(pos);
440 pos = fPlus(is, s);
441
442 fFreeHandle(s);
443 fFreeHandle(t);
444 fFreeHandle(is);
445
446 // vel=vel+accl*acclTimeInterval+randn(n,3)*M_STDDEV_VEL; %??
447
448 t = fDeepCopy(accl);
449 for(i=0; i<(accl->height*accl->width); i++)
450 asubsref(t,i) = asubsref(accl,i) * acclTimeInterval;
451
452 is = fPlus(vel, t);
453
454 fFreeHandle(accl);
455 fFreeHandle(t);
456 s = randnWrapper(n,3);
457 for(i=0; i<(s->height*s->width); i++)
458 {
459 asubsref(s,i) = asubsref(s,i) * M_STDDEV_VEL;
460 }
461
462 fFreeHandle(vel);
463 vel = fPlus(is, s);
464 fFreeHandle(is);
465 fFreeHandle(s);
466 }
467
468 }
469
470 // Self check
471 {
472 F2D* temp;
473 float quatOut=0, velOut=0, posOut=0;
474 int i;
475
476 for(i=0; i<(quat->height*quat->width); i++)
477 quatOut += asubsref(quat, i);
478
479 for(i=0; i<(vel->height*vel->width); i++)
480 velOut += asubsref(vel, i);
481
482 for(i=0; i<(pos->height*pos->width); i++)
483 posOut += asubsref(pos, i);
484
485 subsref(resultMat,0,icount) = quatOut;
486 subsref(resultMat,1,icount) = velOut;
487 subsref(resultMat,2,icount) = posOut;
488 }
489
490 fFreeHandle(sData);
491
492
493 if (asubsref(isEOF,0) == 1)
494 break;
495 }
496 }
497 printf("end..\n");
498
499#ifdef CHECK
500
501 // Self checking - use expected.txt from data directory
502 {
503 int ret=0;
504 float tol = 2.0;
505#ifdef GENERATE_OUTPUT
506 fWriteMatrix(resultMat, argv[1]);
507#endif
508 ret = fSelfCheck(resultMat, "./expected_C.txt", tol);
509 if (ret == -1)
510 printf("Error in Localization\n");
511 }
512 // Self checking done
513#endif
514
515
516 fFreeHandle(STDDEV_GPSPos);
517 iFreeHandle(index);
518 iFreeHandle(sType);
519 iFreeHandle(isEOF);
520 fFreeHandle(fid);
521 fFreeHandle(resultMat);
522 fFreeHandle(pos);
523 fFreeHandle(vel);
524 fFreeHandle(quat);
525 fFreeHandle(ones);
526 fFreeHandle(randW);
527 fFreeHandle(randn);
528 WRITE_TO_FILE
529 return 0;
530
531}
532
533
534
diff --git a/SD-VBS/benchmarks/localization/src/c/weightedSample.c b/SD-VBS/benchmarks/localization/src/c/weightedSample.c
new file mode 100644
index 0000000..28099e8
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/weightedSample.c
@@ -0,0 +1,37 @@
1/********************************
2Author: Sravanthi Kota Venkata
3********************************/
4
5#include <stdio.h>
6#include <stdlib.h>
7#include <math.h>
8#include "localization.h"
9
10I2D* weightedSample(F2D* w)
11{
12 I2D *bin;
13 F2D *seed;
14 int n, i, j;
15
16 n = w->height;
17 seed = randWrapper(n, 1);
18 bin = iSetArray(n, 1, 0);
19
20 for(i=0; i<n; i++)
21 {
22 for(j=0; j<n; j++)
23 {
24 if(asubsref(seed,j) > 0)
25 asubsref(bin,j) = asubsref(bin,j) + 1;
26 }
27 for(j=0; j<n; j++)
28 asubsref(seed,j) = asubsref(seed,j) - asubsref(w,i);
29 }
30
31 free(seed);
32 return bin;
33}
34
35
36
37
generated by cgit v1.2.2 (git 2.25.0) at 2025-04-27 16:45:37 -0400