From f618466c25d43f3bae9e40920273bf77de1e1149 Mon Sep 17 00:00:00 2001 From: leochanj105 Date: Mon, 19 Oct 2020 23:09:30 -0400 Subject: initial sd-vbs initial sd-vbs add sd-vbs sd-vbs --- SD-VBS/benchmarks/localization/src/c/eul2quat.c | 50 ++ .../benchmarks/localization/src/c/generateSample.c | 88 ++++ .../localization/src/c/get3DGaussianProb.c | 49 ++ .../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 | 534 +++++++++++++++++++++ .../benchmarks/localization/src/c/weightedSample.c | 37 ++ .../localization/src/matlab/addEulNoise.m | 12 + .../localization/src/matlab/calculate3DGaussian.m | 11 + .../benchmarks/localization/src/matlab/drawLog.m | 44 ++ .../benchmarks/localization/src/matlab/eul2quat.m | 9 + .../localization/src/matlab/generateSample.m | 25 + .../localization/src/matlab/get3DGaussianProb.m | 22 + .../localization/src/matlab/getGroundData.m | 5 + SD-VBS/benchmarks/localization/src/matlab/mcl.m | 9 + .../benchmarks/localization/src/matlab/mclWhole.m | 18 + .../benchmarks/localization/src/matlab/quat2eul.m | 6 + .../benchmarks/localization/src/matlab/quatConj.m | 5 + .../benchmarks/localization/src/matlab/quatMul.m | 18 + .../benchmarks/localization/src/matlab/quatRot.m | 24 + .../benchmarks/localization/src/matlab/readLoc.m | 53 ++ .../localization/src/matlab/readMatrix.m | 28 ++ .../localization/src/matlab/readSensorData.m | 46 ++ .../localization/src/matlab/script_run_profile.m | 209 ++++++++ .../benchmarks/localization/src/matlab/selfCheck.m | 33 ++ SD-VBS/benchmarks/localization/src/matlab/sumCol.m | 14 + .../localization/src/matlab/weightedSample.m | 20 + 32 files changed, 1671 insertions(+) create mode 100644 SD-VBS/benchmarks/localization/src/c/eul2quat.c create mode 100644 SD-VBS/benchmarks/localization/src/c/generateSample.c create mode 100644 SD-VBS/benchmarks/localization/src/c/get3DGaussianProb.c create mode 100644 SD-VBS/benchmarks/localization/src/c/localization.h create mode 100644 SD-VBS/benchmarks/localization/src/c/mcl.c create mode 100644 SD-VBS/benchmarks/localization/src/c/quat2eul.c create mode 100644 SD-VBS/benchmarks/localization/src/c/quatConj.c create mode 100644 SD-VBS/benchmarks/localization/src/c/quatMul.c create mode 100644 SD-VBS/benchmarks/localization/src/c/quatRot.c create mode 100644 SD-VBS/benchmarks/localization/src/c/readSensorData.c create mode 100644 SD-VBS/benchmarks/localization/src/c/script_localization.c create mode 100644 SD-VBS/benchmarks/localization/src/c/weightedSample.c create mode 100644 SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m create mode 100644 SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m create mode 100644 SD-VBS/benchmarks/localization/src/matlab/drawLog.m create mode 100644 SD-VBS/benchmarks/localization/src/matlab/eul2quat.m create mode 100644 SD-VBS/benchmarks/localization/src/matlab/generateSample.m create mode 100644 SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m create mode 100644 SD-VBS/benchmarks/localization/src/matlab/getGroundData.m create mode 100644 SD-VBS/benchmarks/localization/src/matlab/mcl.m create mode 100644 SD-VBS/benchmarks/localization/src/matlab/mclWhole.m create mode 100644 SD-VBS/benchmarks/localization/src/matlab/quat2eul.m create mode 100644 SD-VBS/benchmarks/localization/src/matlab/quatConj.m create mode 100644 SD-VBS/benchmarks/localization/src/matlab/quatMul.m create mode 100644 SD-VBS/benchmarks/localization/src/matlab/quatRot.m create mode 100644 SD-VBS/benchmarks/localization/src/matlab/readLoc.m create mode 100644 SD-VBS/benchmarks/localization/src/matlab/readMatrix.m create mode 100644 SD-VBS/benchmarks/localization/src/matlab/readSensorData.m create mode 100644 SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m create mode 100644 SD-VBS/benchmarks/localization/src/matlab/selfCheck.m create mode 100644 SD-VBS/benchmarks/localization/src/matlab/sumCol.m create mode 100644 SD-VBS/benchmarks/localization/src/matlab/weightedSample.m (limited to 'SD-VBS/benchmarks/localization/src') 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 +#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 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 +#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 new file mode 100644 index 0000000..bc581c9 --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/c/quat2eul.c @@ -0,0 +1,47 @@ +/******************************** +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 new file mode 100644 index 0000000..3392320 --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/c/script_localization.c @@ -0,0 +1,534 @@ +/******************************** +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]; + + printf("Input txt File: "); + 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); + printf("start\n"); + 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; + } + } + printf("end..\n"); + +#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(STDDEV_GPSPos); + iFreeHandle(index); + iFreeHandle(sType); + iFreeHandle(isEOF); + fFreeHandle(fid); + fFreeHandle(resultMat); + fFreeHandle(pos); + fFreeHandle(vel); + fFreeHandle(quat); + fFreeHandle(ones); + fFreeHandle(randW); + 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 new file mode 100644 index 0000000..28099e8 --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/c/weightedSample.c @@ -0,0 +1,37 @@ +/******************************** +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 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; + + diff --git a/SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m b/SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m new file mode 100644 index 0000000..6a54530 --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m @@ -0,0 +1,22 @@ +function p=get3DGaussianProb(data, mean, A) +n_data=size(data,1); +n_channel=size(data,2); + +p=zeros(n_data,1); +diff=(data)-ones(n_data,1)*mean; +detA = 1; %detA = det(A) +dotA = randWrapper(size(diff,1),1); %dotA = dot(diff*A, diff, 2) +p=sqrt(detA/((2*pi)^n_channel))*exp(-0.5*dotA); + +%% KVS If the above doesnt work, try uncommenting these lines below + +%%temp = (det(A)/((2*pi)^n_channel)); +%temp = (1.0/((2*pi)^n_channel)); +%temp1 = dot(diff*A,diff,2); +%%temp1 = rand(1000,1); +%temp2 = exp(-0.5*temp1); +%p = sqrt(temp) * exp(temp2); +% + + + diff --git a/SD-VBS/benchmarks/localization/src/matlab/getGroundData.m b/SD-VBS/benchmarks/localization/src/matlab/getGroundData.m new file mode 100644 index 0000000..7cf5f9c --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/matlab/getGroundData.m @@ -0,0 +1,5 @@ +function retData=getGroundData(data, tStamp) +idx=find(data(:,1)==tStamp); +retData=data(idx,:); + + diff --git a/SD-VBS/benchmarks/localization/src/matlab/mcl.m b/SD-VBS/benchmarks/localization/src/matlab/mcl.m new file mode 100644 index 0000000..8ab8aa6 --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/matlab/mcl.m @@ -0,0 +1,9 @@ +function [retX, retW]=mcl(x,sData, invCov) +%instead of using importance resampling, I assumed 3D gaussian for each +%particle +%retX=x+randn(nr,nc)*(invCov^-1); %add noise +retX=x; +retW=get3DGaussianProb(retX, sData, invCov); +retW=retW/sum(retW); + + diff --git a/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m b/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m new file mode 100644 index 0000000..4e78b8f --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m @@ -0,0 +1,18 @@ +function retW=mclWhole(quat, Ovel, STDDEV_GPSVel, accl, STDDEV_ACCL) +n=size(quat,1); +OvelNorm=norm(Ovel); +if (OvelNorm>0.5) + Ovel=Ovel/norm(Ovel); + %trick + %quat=addEulNoise(quat, STDDEV_GPSVel); + orgWorld=quatRot([1 0 0],quatConj(quat)); + p1=get3DGaussianProb(orgWorld, Ovel, eye(3)./STDDEV_GPSVel); +else + p1=zeros(n,1); +end + +gravity=quatRot(ones(n,1)*[0 0 -9.8], quat); +p2=get3DGaussianProb(gravity, accl, eye(3)./(STDDEV_ACCL)); +retW=p1+p2; +retW=retW/sum(retW); + \ No newline at end of file diff --git a/SD-VBS/benchmarks/localization/src/matlab/quat2eul.m b/SD-VBS/benchmarks/localization/src/matlab/quat2eul.m new file mode 100644 index 0000000..a6b48ee --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/matlab/quat2eul.m @@ -0,0 +1,6 @@ +function retEul=quat2eul(quat) +retEul=[atan2(2*(quat(:,3).*quat(:,4)+quat(:,1).*quat(:,2))... + ,(quat(:,1).^2 -quat(:,2).^2 -quat(:,3).^2 +quat(:,4).^2)) ... + asin(-2*(quat(:,2).*quat(:,4)-quat(:,1).*quat(:,3))) ... + atan2(2*(quat(:,2).*quat(:,3)+quat(:,1).*quat(:,4))... + ,(quat(:,1).^2 +quat(:,2).^2 -quat(:,3).^2 -quat(:,4).^2))]; diff --git a/SD-VBS/benchmarks/localization/src/matlab/quatConj.m b/SD-VBS/benchmarks/localization/src/matlab/quatConj.m new file mode 100644 index 0000000..ff494b4 --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/matlab/quatConj.m @@ -0,0 +1,5 @@ +function retQuat=quatConj(a) +rows = size(a,1); +retQuat = zeros(rows, 4); +retQuat=[a(:,1), -a(:,2), -a(:,3), -a(:,4)]; + diff --git a/SD-VBS/benchmarks/localization/src/matlab/quatMul.m b/SD-VBS/benchmarks/localization/src/matlab/quatMul.m new file mode 100644 index 0000000..b397554 --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/matlab/quatMul.m @@ -0,0 +1,18 @@ +function retQuat=quatMul(a, b) + +rowsa = size(a,1); +colsa = size(a,2); + +rowsb = size(b,1); +colsb = size(b,2); + +retQuat=[a(:,1).*b(:,1) - a(:,2).*b(:,2) - a(:,3).*b(:,3) - a(:,4).*b(:,4) ... + a(:,1).*b(:,2) + a(:,2).*b(:,1) + a(:,3).*b(:,4) - a(:,4).*b(:,3) ... + a(:,1).*b(:,3) - a(:,2).*b(:,4) + a(:,3).*b(:,1) + a(:,4).*b(:,2) ... + a(:,1).*b(:,4) + a(:,2).*b(:,3) - a(:,3).*b(:,2) + a(:,4).*b(:,1)]; + +% disp('retQuat'); +% disp(retQuat); + + + 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) + + + + diff --git a/SD-VBS/benchmarks/localization/src/matlab/readLoc.m b/SD-VBS/benchmarks/localization/src/matlab/readLoc.m new file mode 100644 index 0000000..fc679d4 --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/matlab/readLoc.m @@ -0,0 +1,53 @@ +function out = readLoc +out = [... +10.512346 3.466955 9.006616 ;... +13.242645 4.280358 14.675757 ;... +12.418589 6.286052 22.721924 ;... +13.598760 5.229711 19.622804 ;... +13.568058 5.229711 19.622804 ;... +13.537888 5.229711 19.622804 ;... +13.508315 5.229711 19.622804 ;... +13.479378 5.229711 19.622804 ;... +13.450420 5.229711 19.622804 ;... +13.345976 7.607621 29.644342 ;... +13.500197 7.803792 32.248338 ;... +13.354279 10.248983 42.939018 ;... +13.500197 10.442337 45.593042 ;... +13.354279 12.925516 56.404386 ;... +13.700372 10.538259 47.007521 ;... +13.669945 10.538259 47.007521 ;... +13.640058 10.538259 47.007521 ;... +13.608179 10.538259 47.007521 ;... +13.576901 10.538259 47.007521 ;... +13.545771 10.538259 47.007521 ;... +13.731515 12.682360 55.887526 ;... +1.016192 12.058634 55.442021 ;... +6.263918 15.094210 64.436498 ;... +6.263918 18.208480 70.198939 ;... +6.263918 20.848498 78.618094 ;... +6.263918 23.887747 84.333007 ;... +6.263918 22.065907 78.853015 ;... +6.237774 22.065907 78.853015 ;... +6.211166 22.065907 78.853015 ;... +6.183471 22.065907 78.853015 ;... +6.155114 22.065907 78.853015 ;... +6.126730 22.065907 78.853015 ;... +6.154098 26.480108 89.543387 ;... +6.168298 28.571946 93.679254 ;... +6.157243 32.868572 105.963003 ;... +6.155032 35.245287 110.401650 ;... +6.155032 38.531472 119.557291 ;... +6.155032 36.849296 111.296948 ;... +6.128369 36.849296 111.296948 ;... +6.101609 36.849296 111.296948 ;... +6.073750 36.849296 111.296948 ;... +6.046859 36.849296 111.296948 ;... +6.019815 36.849296 111.296948 ;... +6.046273 40.863016 123.894104 ;... +6.060599 43.054591 127.680881 ;... +6.049203 47.179478 140.858302 ;... +6.046924 49.530293 145.484328 ;... +6.046924 52.526113 153.189673 ;... +6.046924 51.113294 146.241709 ;... +6.021072 51.113294 146.241709 ... +]; \ No newline at end of file diff --git a/SD-VBS/benchmarks/localization/src/matlab/readMatrix.m b/SD-VBS/benchmarks/localization/src/matlab/readMatrix.m new file mode 100644 index 0000000..fe684c5 --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/matlab/readMatrix.m @@ -0,0 +1,28 @@ +function readMatrix(srcImage, outName) + +write = fopen([outName '.m'], 'w'); + +count = fwrite(write, 'function out = '); +count = fwrite(write, outName); +fprintf(write, '\n'); +count = fwrite(write, 'out = [...'); +fprintf(write, '\n'); + +height = size(srcImage,1); +width = size(srcImage,2); + +for nI=1:height + for nJ=1:width + fprintf(write, '%f ', srcImage(nI,nJ)); + end + if(nI < height) + fprintf(write, ';...\n'); + end +end + +fprintf(write, '...\n'); +count = fwrite(write, '];'); + +fclose(write); + +end diff --git a/SD-VBS/benchmarks/localization/src/matlab/readSensorData.m b/SD-VBS/benchmarks/localization/src/matlab/readSensorData.m new file mode 100644 index 0000000..e84a6db --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/matlab/readSensorData.m @@ -0,0 +1,46 @@ +function [retTStamp, retType, retData, retEOF, index]=readSensorData(index1, fid) + +index = index1+1; +rows = size(fid, 1); +retTStamp = 0; +retType = 0; +retData = zeros(1,8); + +if(index > rows) + retEOF = 1; +else +% for i=index:rows +% index = i; +% if(fid(i,2) == 4) +% break; +% end +% end + if(index == rows) + retEOF = 1; + else + retEOF = 0; + end + + k = index; + retTStamp=fid(k,1); + retType=fid(k,2); + if(fid(k, 2) == 1 || fid(k, 2) == 2 || fid(k, 2) == 3) + index = k; + for i=1:3 + retData(1,i)=fid(k,i+2); +% fprintf(1,'retData,i -> %f\t%d\n', retData(1,i), i); + end + end + if(fid(k, 2) == 4) + index = k; + for i=1:3 + retData(1,i)=fid(k,i+2); +% fprintf(1,'retData,i -> %f\t%d\n', retData(1,i), i); + end + for i=4:8 + retData(1,i) = fid(k+1,i-3); +% fprintf(1,'retData,i -> %f\t%d\n', retData(1,i), i); + end + index = index + 1; + end +end diff --git a/SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m b/SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m new file mode 100644 index 0000000..44ce3aa --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m @@ -0,0 +1,209 @@ +function script_run_profile(dataDir, resultDir, type, common, toolDir) + +path(path, common); +file = fopen([dataDir, '/1.txt'], 'r'); +full = fscanf(file,'%f'); +elapsed = zeros(1,2); + +rows = full(2); +cols = full(1); +fid = zeros(rows, cols); + +k = 3; +for i=1:rows + for j =1:cols + fid(i,j) = full(k); + k = k+1; + end +end + +fclose(file); + +n=1000; + +gyroTimeInterval=0.01; +acclTimeInterval=0.01; + +STDDEV_GPSVel=0.5; + +STDDEV_ODOVel=0.1; +STDDEV_ACCL=1; +M_STDDEV_GYRO=0.1; +M_STDDEV_POS=0.1; +M_STDDEV_VEL=0.02; + +if(strcmp(type,'test')) + n = 3; + gyroTimeInterval = 0.1; + acclTimeInterval = 0.1; + M_STDDEV_VEL = 0.2; + +elseif(strcmp(type, 'sim_fast')) + n = 3; +elseif(strcmp(type, 'sim')) + n = 10; +elseif(strcmp(type, 'sqcif')) + n = 800; +elseif(strcmp(type, 'qcif')) + n = 500; +elseif(strcmp(type, 'vga')) + n = 2000; +elseif(strcmp(type, 'wuxga')) + n = 3000; +end + +fprintf(1,'Input size\t\t- (%dx%dx%d)\n', rows, cols,n); +pos=zeros(n,3); + +vel=zeros(n,3) + randWrapper(n,3)*STDDEV_ODOVel; +pi = 3.1416; + +eul1 = eul2quat([zeros(n,2), randWrapper(n,1)*2*pi]); +eul2 = eul2quat([pi, 0, 0]); +quat=quatMul(eul1, eul2); + +i=0; +index = 0; +resultMat = zeros(3,rows); + +while 1 + i=i+1; + [tStamp, sType, sData, isEOF, index] = readSensorData(index, fid); + + start = photonStartTiming; + + if(sType==2) + + + %Motion model + gyro = sData(1, 1:3); + randnWrapper_mat = randnWrapper(n,3); % KVS: We do not have function implementation for randnWrapper() + gyro=ones(n,1)*gyro+randnWrapper_mat*M_STDDEV_GYRO; + + abc = gyro.^2; + abcd = sumCol(abc); + norm_gyro = sqrt(abcd); +% norm_gyro=sqrt(sum(gyro.^2,2)); + angleAlpha=norm_gyro*gyroTimeInterval; + quatDelta=[cos(angleAlpha/2), gyro./(norm_gyro*ones(1,3)+0.00000001).*(sin(angleAlpha/2)*ones(1,3))]; + quat=quatMul(quat, quatDelta); + + end + + if(sType==4) + %Observation + STDDEV_GPSPos=[sData(1,7), 0, 0; 0, sData(1,8), 0; 0, 0, 15]; + Opos=sData(1,1:3); + + %Initialize + + randnWrapper_mat = randnWrapper(n,3); + if sum(sum(pos))==0 + pos=ones(n,1) * Opos + randnWrapper_mat*STDDEV_GPSPos; + else + rows = size(STDDEV_GPSPos, 1); + cols = size(STDDEV_GPSPos, 2); + + temp_STDDEV_GPSPos = ones(rows,cols); + for mnrows=1:rows % KVS" Photon rejects this loop becasue of too many nestings ?? + for mncols=1:cols + temp_STDDEV_GPSPos(mnrows, mncols) = power(STDDEV_GPSPos(mnrows,mncols),-1); + end + end + [temp, w]=mcl(pos, Opos , temp_STDDEV_GPSPos); + [quat, vel, pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS); + end + + %compare direction + Ovel=sData(1,4:6); + +% KVS: We do not have function for norm() yet! So replacing this operating with OvelNorm + +% OvelNorm=norm(Ovel); + OvelNorm= 2.0; %1.1169e+09; + + if (OvelNorm>0.5) + + % KVS: Similar here: No impln of norm(), so replacing + % norm(Ovel) with OvelNorm value + + Ovel=Ovel/OvelNorm; + %trick + %quat=addEulNoise(quat, STDDEV_GPSVel); + qConj = quatConj(quat); + orgWorld=quatRot([1, 0, 0],qConj); + eye3 = [1,0,0;0,1,0;0,0,1]; + [temp, w]=mcl(orgWorld, Ovel, eye3./STDDEV_GPSVel); + [quat, vel, pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS); + end + + end + + if(sType==1) + + %Observation + Ovel=sData(1,1); + [temp, w]=mcl(sqrt(vel(:,1).^2+vel(:,2).^2+vel(:,3).^2), Ovel, 1/(STDDEV_ODOVel)); + + [quat vel pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS); + end + + if(sType==3) + %Observation + accl=sData(1,1:3); + + gtemp = ones(n,1) * [0 0 -9.8]; + + gravity=quatRot(gtemp, quat); + eye3 = [1,0,0;0,1,0;0,0,1]; + [gravity, w]=mcl(gravity, accl, eye3./(STDDEV_ACCL)); + + [quat, vel, pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS); + + %Motion model + accl=ones(n,1)*accl; + accl=accl-gravity; + pos=pos+quatRot(vel,quatConj(quat))*acclTimeInterval ... + +1/2*quatRot(accl,quatConj(quat))*acclTimeInterval^2 ... + +randnWrapper(n,3)*M_STDDEV_POS; + vel=vel+accl*acclTimeInterval+randnWrapper(n,3)*M_STDDEV_VEL; + + end + + stop = photonEndTiming; + + temp = photonReportTiming(start, stop); + elapsed(1) = elapsed(1) + temp(1); + elapsed(2) = elapsed(2) + temp(2); + + % Self check + + quatOut = 0; + posOut = 0; + velOut = 0; + + for ij=1:(size(quat,1)*size(quat,2)) + quatOut = quatOut + quat(ij); + end + + for ij=1:(size(vel,1)*size(vel,2)) + velOut = velOut + vel(ij); + end + + for ij=1:(size(pos,1)*size(pos,2)) + posOut = posOut + pos(ij); + end + + resultMat(:, i) = [quatOut, velOut, posOut]; + + if (isEOF == 1) + break; + end +end + +%% Timing +photonPrintTiming(elapsed); + +%% Self checking %% +fWriteMatrix(resultMat,dataDir); + diff --git a/SD-VBS/benchmarks/localization/src/matlab/selfCheck.m b/SD-VBS/benchmarks/localization/src/matlab/selfCheck.m new file mode 100644 index 0000000..ac80ebd --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/matlab/selfCheck.m @@ -0,0 +1,33 @@ +function ret = selfCheck(in1, in2, tol) + +r1 = size(in1, 1); +c1 = size(in1, 2); + +r2 = size(in2, 1); +c2 = size(in2, 2); + +ret = 1; + +if r1~=r2 + disp(1298); + ret = 0; +end + +if c1 ~= c2 + disp(1297); + ret = 0; +end + +for i=1:r1 + if(ret == 0) + break; + end + for j=1:c1 + if( abs(in1(i,j)-in2(i,j)) > tol) + ret = 0; + break; + end + end +end + + diff --git a/SD-VBS/benchmarks/localization/src/matlab/sumCol.m b/SD-VBS/benchmarks/localization/src/matlab/sumCol.m new file mode 100644 index 0000000..ad0114e --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/matlab/sumCol.m @@ -0,0 +1,14 @@ +function ret = sumCol(mat) + +row = size(mat, 1); +col = size(mat, 2); + +ret = zeros(row, 1); + +for i=1:row + temp = 0; + for j=1:col + temp = temp + mat(i, j); + end + ret(i, 1) = temp; +end diff --git a/SD-VBS/benchmarks/localization/src/matlab/weightedSample.m b/SD-VBS/benchmarks/localization/src/matlab/weightedSample.m new file mode 100644 index 0000000..dc9274b --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/matlab/weightedSample.m @@ -0,0 +1,20 @@ +%function x_gen_id=weightedSample(w) +function bin=weightedSample(w) +n=size(w,1); +seed=randWrapper(n,1); +bin = zeros(n,1); +%x_gen_id=zeros(n,1); + for i=1:n + for j=1:n + if(seed(j,1) > 0) + bin(j,1) = bin(j,1) + 1; +% x_gen_id(j,1) = x_gen_id(j,1) + bin(j,1); + end + end +% bin = (seed>0); +% x_gen_id=x_gen_id+bin; + seed=seed-w(i,1); + end +% x_gen_id = bin; +%x_gen=x(:,x_gen_id); + -- cgit v1.2.2