From 14c854543b1a3cf344a371a5b45595657f95786b Mon Sep 17 00:00:00 2001 From: leochanj105 Date: Tue, 26 Oct 2021 13:22:32 -0400 Subject: add deadlines and costs --- SD-VBS/benchmarks/localization/src/c/eul2quat.c | 50 -- .../benchmarks/localization/src/c/generateSample.c | 88 ---- .../localization/src/c/get3DGaussianProb.c | 49 -- SD-VBS/benchmarks/localization/src/c/input.txt | 8 - .../benchmarks/localization/src/c/localization.h | 24 - SD-VBS/benchmarks/localization/src/c/mcl.c | 34 -- SD-VBS/benchmarks/localization/src/c/quat2eul.c | 47 -- SD-VBS/benchmarks/localization/src/c/quatConj.c | 32 -- SD-VBS/benchmarks/localization/src/c/quatMul.c | 55 --- SD-VBS/benchmarks/localization/src/c/quatRot.c | 47 -- .../benchmarks/localization/src/c/readSensorData.c | 63 --- .../localization/src/c/script_localization.c | 525 --------------------- .../benchmarks/localization/src/c/weightedSample.c | 37 -- 13 files changed, 1059 deletions(-) delete mode 100644 SD-VBS/benchmarks/localization/src/c/eul2quat.c delete mode 100644 SD-VBS/benchmarks/localization/src/c/generateSample.c delete mode 100644 SD-VBS/benchmarks/localization/src/c/get3DGaussianProb.c delete mode 100644 SD-VBS/benchmarks/localization/src/c/input.txt delete mode 100644 SD-VBS/benchmarks/localization/src/c/localization.h delete mode 100644 SD-VBS/benchmarks/localization/src/c/mcl.c delete mode 100644 SD-VBS/benchmarks/localization/src/c/quat2eul.c delete mode 100644 SD-VBS/benchmarks/localization/src/c/quatConj.c delete mode 100644 SD-VBS/benchmarks/localization/src/c/quatMul.c delete mode 100644 SD-VBS/benchmarks/localization/src/c/quatRot.c delete mode 100644 SD-VBS/benchmarks/localization/src/c/readSensorData.c delete mode 100644 SD-VBS/benchmarks/localization/src/c/script_localization.c delete mode 100644 SD-VBS/benchmarks/localization/src/c/weightedSample.c (limited to 'SD-VBS/benchmarks/localization/src/c') diff --git a/SD-VBS/benchmarks/localization/src/c/eul2quat.c b/SD-VBS/benchmarks/localization/src/c/eul2quat.c deleted file mode 100644 index bea75cf..0000000 --- a/SD-VBS/benchmarks/localization/src/c/eul2quat.c +++ /dev/null @@ -1,50 +0,0 @@ -/******************************** -Author: Sravanthi Kota Venkata -********************************/ - -#include -#include -#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 -#include -#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; iwidth; j++) - { - subsref(retQuat,i,j) = subsref(quat,index,j); - } - } - - for(i=0; iwidth; j++) - { - subsref(retVel,i,j) = subsref(vel,index,j); - } - } - - for(i=0; iwidth; j++) - { - subsref(retPos,i,j) = subsref(pos,index,j); - } - } - - for(i=0; iheight; i++) - { - for(j=0; jwidth; j++) - { - subsref(quat,i,j) = subsref(retQuat,i,j); - } - } - - for(i=0; iheight; i++) - { - for(j=0; jwidth; j++) - { - subsref(vel,i,j) = subsref(retVel,i,j); - } - } - - for(i=0; iheight; i++) - { - for(j=0; jwidth; 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 deleted file mode 100644 index 58c4475..0000000 --- a/SD-VBS/benchmarks/localization/src/c/get3DGaussianProb.c +++ /dev/null @@ -1,49 +0,0 @@ -/******************************** -Author: Sravanthi Kota Venkata -********************************/ - -#include -#include -#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 -#include -#include "localization.h" - -F2D* mcl(F2D* x, F2D* sData, F2D* invConv) -{ - int i, j; - F2D *retW, *retX, *sum; - float sumVal; - - retX = fDeepCopy(x); - retW = get3DGaussianProb(retX, sData, invConv); - sum = fSum(retW); - if(sum->height == 1 && sum->width ==1) - { - sumVal = asubsref(sum,0); - for(i=0; iheight; i++) - for(j=0; jwidth; j++) - subsref(retW,i,j) = subsref(retW,i,j)/sumVal; - } - else - retW = fMdivide(retW, sum); - - fFreeHandle(retX); - fFreeHandle(sum); - - return retW; -} - - diff --git a/SD-VBS/benchmarks/localization/src/c/quat2eul.c b/SD-VBS/benchmarks/localization/src/c/quat2eul.c deleted file mode 100644 index bc581c9..0000000 --- a/SD-VBS/benchmarks/localization/src/c/quat2eul.c +++ /dev/null @@ -1,47 +0,0 @@ -/******************************** -Author: Sravanthi Kota Venkata -********************************/ - -#include -#include -#include "localization.h" -#include - -F2D* quat2eul(F2D* quat) -{ - F2D *retEul; - int i, j, k; - int rows, cols; - - rows = quat->height; - cols = quat->width; - - retEul = fSetArray(rows, 3, 0); - - for(i=0; i -#include -#include "localization.h" - -F2D* quatConj(F2D* a) -{ - F2D* retQuat; - int rows, cols; - int i, j, k; - - rows = a->height; - cols = a->width; - retQuat = fSetArray(rows, 4, 0); - - for(i=0; i -#include -#include "localization.h" - -F2D* quatMul(F2D* a, F2D* b) -{ - int ra, ca, rb, cb; - F2D *ret; - int i, j, k=0; - - ra = a->height; - ca = a->width; - - rb = b->height; - cb = b->width; - - ret = fSetArray(ra, 4, 0); - - j = 0; - for(i=0; i -#include -#include "localization.h" - -F2D* quatRot(F2D* vec, F2D* rQuat) -{ - F2D *ret; - int nr, i, j, k, rows, cols; - F2D *tv, *vQuat, *temp, *temp1; - F2D *retVec; - - nr = vec->height; - tv = fSetArray(nr, 1, 0); - vQuat = fHorzcat(tv, vec); - temp = quatMul(rQuat, vQuat); - temp1 = quatConj(rQuat); - retVec = quatMul(temp, temp1); - - rows = retVec->height; - cols = retVec->width; - - ret = fSetArray(rows, 3, 0); - - for(i=0; i -#include -#include "localization.h" - -F2D* readSensorData(I2D* index, F2D* fid, I2D* type, I2D* eof) -{ - F2D *retData; - int rows, i, j, k; - int atype=-1, aindex; - - aindex = asubsref(index, 0); - - asubsref(index,0) = asubsref(index,0) + 1; - rows = fid->height; - asubsref(type,0) = 0; - retData = fSetArray(1, 8, 0); - - if( asubsref(index,0) > (rows-1) ) - asubsref(eof,0) = 1; - else - { - if( asubsref(index,0) == rows) - asubsref(eof,0) = 1; - else - asubsref(eof,0) = 0; - - k = asubsref(index,0); - atype = subsref(fid, k, 1); - if( (atype == 1) || (atype == 2) || (atype == 3) ) - { - for(i=0; i<3; i++) - { - asubsref(retData,i) = subsref(fid,k,(i+2)); - } - } - if( atype == 4 ) - { - for(i=0; i<3; i++) - { - asubsref(retData,i) = subsref(fid,k,(i+2)); - } - for(i=3; i<8; i++) - { - asubsref(retData,i) = subsref(fid,k+1,(i-3)); - } - aindex = aindex + 1; - } - aindex = aindex + 1; - } - - asubsref(index,0) = aindex; - asubsref(type, 0) = atype; - - return retData; -} - - - - diff --git a/SD-VBS/benchmarks/localization/src/c/script_localization.c b/SD-VBS/benchmarks/localization/src/c/script_localization.c deleted file mode 100644 index ddd2215..0000000 --- a/SD-VBS/benchmarks/localization/src/c/script_localization.c +++ /dev/null @@ -1,525 +0,0 @@ -/******************************** -Author: Sravanthi Kota Venkata -********************************/ - -#include -#include -#include -#include "localization.h" -#include "extra.h" -#define LOCOLIZATION_MEM 1<<24 -int main(int argc, char* argv[]) -{ - SET_UP - mallopt(M_TOP_PAD, LOCOLIZATION_MEM); - mallopt(M_MMAP_MAX, 0); - int n, i, j, k, icount=-1; - F2D* fid; - float gyroTimeInterval=0.01; - float acclTimeInterval=0.01; - - float STDDEV_GPSVel=0.5; - float STDDEV_ODOVel=0.1; - float STDDEV_ACCL=1; - float M_STDDEV_GYRO=0.1; - float M_STDDEV_POS=0.1; - float M_STDDEV_VEL=0.02; - - F2D *pos, *vel; - float pi = 3.1416; - F2D *eul1, *eul2, *quat; - F2D *sData, *gyro, *norm_gyro, *angleAlpha; - F2D *quatDelta, *Opos, *temp_STDDEV_GPSPos, *w; - F2D *qConj, *orgWorld, *accl, *gtemp; - F2D *gravity, *t1; - I2D *tStamp, *sType, *isEOF; - I2D *index; - int rows, cols; - F2D *resultMat; - F2D *STDDEV_GPSPos; - F2D *ones, *randW; - - char im1[100]; - - scanf("%s", im1); - - fid = readFile(im1); - - n = 1000; - - #ifdef test - n = 3; - gyroTimeInterval = 0.1; - acclTimeInterval = 0.1; - M_STDDEV_VEL = 0.2; - #endif - #ifdef sim_fast - n = 3; - #endif - #ifdef sim - n = 10; - #endif - #ifdef sqcif - n = 800; - #endif - #ifdef qcif - n = 500; - #endif - #ifdef vga - n = 2000; - #endif - #ifdef wuxga - n = 3000; - #endif - - resultMat = fSetArray(3,fid->height, 0); - - pos = fSetArray(n, 3, 0); - vel = fSetArray(n, 3, 0); - ones = fSetArray(n,1,1); - - - F2D *randn; - - randn = randWrapper(n,3); - for_each_job { - fResetArray(pos,n, 3, 0); - fResetArray(vel,n, 3, 0); - fResetArray(ones,n,1,1); - - { - int j; - for(i=0; iheight, gyro->width); - t1 = fDeepCopy(randW); - - for(i=0; i<(n*3); i++) - { - asubsref(t1,i) = asubsref(randW,i) * M_STDDEV_GYRO; - asubsref(gyro, i) += asubsref(t1,i); - asubsref(abc, i) = pow(asubsref(gyro, i), 2); - } - fFreeHandle(t1); - abcd = fSum2(abc, 2); - - norm_gyro = fMallocHandle(abcd->height,abcd->width); - angleAlpha = fMallocHandle(abcd->height, abcd->width); - - for(i=0; i<(abcd->height*abcd->width); i++) - { - asubsref(norm_gyro, i) = sqrt(asubsref(abcd,i)); - asubsref(angleAlpha,i) = asubsref(norm_gyro,i) * gyroTimeInterval; - } - - qD_r += angleAlpha->height + gyro->height; - qD_c += angleAlpha->width + 3; - - fFreeHandle(t); - fFreeHandle(abcd); - - cosA = fSetArray(angleAlpha->height, angleAlpha->width, 0); - sinA = fSetArray(angleAlpha->height, angleAlpha->width, 0); - - for(i=0; i<(cosA->height*cosA->width); i++) - asubsref(cosA,i) = cos( asubsref(angleAlpha,i) /2 ); - - for(i=0; i<(sinA->height*sinA->width); i++) - asubsref(sinA,i) = sin( asubsref(angleAlpha,i) /2 ); - - - fFreeHandle(abc); - abc = fSetArray(1,3,1); - t1 = fMtimes(norm_gyro, abc); - t = ffDivide(gyro, t1); - fFreeHandle(t1); - - abcd = fMtimes(sinA, abc); - t1 = fTimes(t, abcd); - quatDelta = fHorzcat(cosA, t1); - - fFreeHandle(abcd); - fFreeHandle(t); - fFreeHandle(t1); - fFreeHandle(abc); - - t = quatMul(quat, quatDelta); - fFreeHandle(quat); - fFreeHandle(quatDelta); - quat = fDeepCopy(t); - - fFreeHandle(t); - fFreeHandle(norm_gyro); - fFreeHandle(gyro); - fFreeHandle(angleAlpha); - fFreeHandle(cosA); - fFreeHandle(sinA); - } - } - - if( asubsref(sType,0) ==4) - { - //Observation - float tempSum=0; - F2D *Ovel; - float OvelNorm; - int i; - - asubsref(STDDEV_GPSPos, 0) = asubsref(sData, 6); - asubsref(STDDEV_GPSPos, 4) = asubsref(sData,7); - asubsref(STDDEV_GPSPos, 8) = 15; - - Opos = fDeepCopyRange(sData, 0, 1, 0, 3); - - //Initialize - for(i=0; i<(pos->height*pos->width); i++) - tempSum += asubsref(pos,i); - - if(tempSum == 0) - { - F2D *t, *t1; - t = fMtimes( randW, STDDEV_GPSPos); - t1 = fMtimes(ones, Opos); - - for(i=0; i<(pos->height*pos->width); i++) - asubsref(pos,i) = asubsref(t,i) + asubsref(t1,i); - - fFreeHandle(t); - fFreeHandle(t1); - } - else - { - int rows, cols; - int mnrows, mncols; - - rows = STDDEV_GPSPos->height; - cols = STDDEV_GPSPos->width; - - temp_STDDEV_GPSPos = fSetArray(rows,cols,1); - for( mnrows=0; mnrows0.5) - { - F2D *t; - t = fDeepCopy(Ovel); - fFreeHandle(Ovel); - /* This is a double precision division */ - Ovel = fDivide(t, OvelNorm); - qConj = quatConj(quat); - fFreeHandle(t); - - { - t = fSetArray(1,3,0); - subsref(t,0,0) = 1; - orgWorld = quatRot(t, qConj); - fFreeHandle(t); - fFreeHandle(qConj); - t = fSetArray(3,3,0); - asubsref(t,0) = 1; - asubsref(t,4) = 1; - asubsref(t,8) = 1; - - { - int i; - for(i=0; i<(t->height*t->width); i++) - asubsref(t, i) = asubsref(t,i)/STDDEV_GPSVel; - w = mcl( orgWorld, Ovel, t); - generateSample(w, quat, vel, pos); - } - - fFreeHandle(t); - fFreeHandle(w); - fFreeHandle(orgWorld); - } - } - fFreeHandle(Ovel); - } - - if( asubsref(sType,0) ==1) - { - //Observation - F2D *Ovel; - F2D *t, *t1, *t2; - float valVel; - - t = fSetArray(vel->height, 1, 0); - - for(i=0; iheight; i++) - { - subsref(t,i,0) = sqrt(pow(subsref(vel,i,0),2) + pow(subsref(vel,i,1),2) + pow(subsref(vel,i,2),2)); - } - - Ovel = fSetArray(1, 1, asubsref(sData,0)); - valVel = 1.0/STDDEV_ODOVel; - - t1 = fSetArray(1,1,(1.0/STDDEV_ODOVel)); - w = mcl (t, Ovel, t1); - generateSample(w, quat, vel, pos); - - - fFreeHandle(w); - fFreeHandle(t); - fFreeHandle(t1); - fFreeHandle(Ovel); - } - - if( asubsref(sType,0) ==3) - { - //Observation - F2D *t; - t = fSetArray(1, 3, 0); - asubsref(t,2) = -9.8; - - accl = fDeepCopyRange(sData, 0, 1, 0, 3); - gtemp = fMtimes( ones, t); - gravity = quatRot(gtemp, quat); - - fFreeHandle(gtemp); - fFreeHandle(t); - t = fSetArray(3,3,0); - asubsref(t,0) = 1; - asubsref(t,4) = 1; - asubsref(t,8) = 1; - - { - int i; - for(i=0; i<(t->height*t->width); i++) - asubsref(t,i) = asubsref(t,i)/STDDEV_ACCL; - w = mcl( gravity, accl, t); - } - - generateSample(w, quat, vel, pos); - fFreeHandle(t); - //Motion model - t = fMtimes(ones, accl); - fFreeHandle(accl); - - accl = fMinus(t, gravity); - - fFreeHandle(w); - fFreeHandle(gravity); - fFreeHandle(t); - - { - //pos=pos+quatRot(vel,quatConj(quat))*acclTimeInterval+1/2*quatRot(accl,quatConj(quat))*acclTimeInterval^2+randn(n,3)*M_STDDEV_POS; - - F2D *s, *is; - int i; - is = quatConj(quat); - s = quatRot(vel, is); - fFreeHandle(is); - - for(i=0; i<(s->height*s->width); i++) - { - asubsref(s,i) = asubsref(s,i)*acclTimeInterval; //+(1/2); - } - is = fPlus(pos, s); - fFreeHandle(pos); - pos = fDeepCopy(is); - fFreeHandle(is); - fFreeHandle(s); - - /** pos_ above stores: pos+quatRot(vel,quatConj(quat))*acclTimeInterval **/ - - is = quatConj(quat); - s = quatRot(accl, is); - t = fDeepCopy(s); - for(i=0; i<(s->height*s->width); i++) - { - asubsref(t,i) = 1/2*asubsref(s,i)*acclTimeInterval*acclTimeInterval; - } - - /** t_ above stores: 1/2*quatRot(accl,quatCong(quat))*acclTimeInterval^2 **/ - - fFreeHandle(s); - fFreeHandle(is); - s = randnWrapper(n,3); - - for(i=0; i<(s->height*s->width); i++) - { - asubsref(s,i) = asubsref(s,i) * M_STDDEV_POS; - } - - /** s_ above stores: randn(n,3)*M_STDDEV_POS **/ - - is = fPlus(pos, t); - fFreeHandle(pos); - pos = fPlus(is, s); - - fFreeHandle(s); - fFreeHandle(t); - fFreeHandle(is); - - // vel=vel+accl*acclTimeInterval+randn(n,3)*M_STDDEV_VEL; %?? - - t = fDeepCopy(accl); - for(i=0; i<(accl->height*accl->width); i++) - asubsref(t,i) = asubsref(accl,i) * acclTimeInterval; - - is = fPlus(vel, t); - - fFreeHandle(accl); - fFreeHandle(t); - s = randnWrapper(n,3); - for(i=0; i<(s->height*s->width); i++) - { - asubsref(s,i) = asubsref(s,i) * M_STDDEV_VEL; - } - - fFreeHandle(vel); - vel = fPlus(is, s); - fFreeHandle(is); - fFreeHandle(s); - } - - } - - // Self check - { - F2D* temp; - float quatOut=0, velOut=0, posOut=0; - int i; - - for(i=0; i<(quat->height*quat->width); i++) - quatOut += asubsref(quat, i); - - for(i=0; i<(vel->height*vel->width); i++) - velOut += asubsref(vel, i); - - for(i=0; i<(pos->height*pos->width); i++) - posOut += asubsref(pos, i); - - subsref(resultMat,0,icount) = quatOut; - subsref(resultMat,1,icount) = velOut; - subsref(resultMat,2,icount) = posOut; - } - - fFreeHandle(sData); - - if (asubsref(isEOF,0) == 1) - break; - } - fFreeHandle(STDDEV_GPSPos); - iFreeHandle(index); - iFreeHandle(sType); - iFreeHandle(isEOF); - fFreeHandle(quat); - fFreeHandle(randW); - } - -#ifdef CHECK - - // Self checking - use expected.txt from data directory - { - int ret=0; - float tol = 2.0; -#ifdef GENERATE_OUTPUT - fWriteMatrix(resultMat, argv[1]); -#endif - ret = fSelfCheck(resultMat, "./expected_C.txt", tol); - if (ret == -1) - printf("Error in Localization\n"); - } - // Self checking done -#endif - - fFreeHandle(fid); - fFreeHandle(resultMat); - fFreeHandle(pos); - fFreeHandle(vel); - fFreeHandle(ones); - fFreeHandle(randn); - WRITE_TO_FILE - return 0; - -} - - - diff --git a/SD-VBS/benchmarks/localization/src/c/weightedSample.c b/SD-VBS/benchmarks/localization/src/c/weightedSample.c deleted file mode 100644 index 28099e8..0000000 --- a/SD-VBS/benchmarks/localization/src/c/weightedSample.c +++ /dev/null @@ -1,37 +0,0 @@ -/******************************** -Author: Sravanthi Kota Venkata -********************************/ - -#include -#include -#include -#include "localization.h" - -I2D* weightedSample(F2D* w) -{ - I2D *bin; - F2D *seed; - int n, i, j; - - n = w->height; - seed = randWrapper(n, 1); - bin = iSetArray(n, 1, 0); - - for(i=0; i 0) - asubsref(bin,j) = asubsref(bin,j) + 1; - } - for(j=0; j