diff options
Diffstat (limited to 'SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m')
| -rw-r--r-- | SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m | 209 |
1 files changed, 0 insertions, 209 deletions
diff --git a/SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m b/SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m deleted file mode 100644 index 44ce3aa..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m +++ /dev/null | |||
| @@ -1,209 +0,0 @@ | |||
| 1 | function script_run_profile(dataDir, resultDir, type, common, toolDir) | ||
| 2 | |||
| 3 | path(path, common); | ||
| 4 | file = fopen([dataDir, '/1.txt'], 'r'); | ||
| 5 | full = fscanf(file,'%f'); | ||
| 6 | elapsed = zeros(1,2); | ||
| 7 | |||
| 8 | rows = full(2); | ||
| 9 | cols = full(1); | ||
| 10 | fid = zeros(rows, cols); | ||
| 11 | |||
| 12 | k = 3; | ||
| 13 | for i=1:rows | ||
| 14 | for j =1:cols | ||
| 15 | fid(i,j) = full(k); | ||
| 16 | k = k+1; | ||
| 17 | end | ||
| 18 | end | ||
| 19 | |||
| 20 | fclose(file); | ||
| 21 | |||
| 22 | n=1000; | ||
| 23 | |||
| 24 | gyroTimeInterval=0.01; | ||
| 25 | acclTimeInterval=0.01; | ||
| 26 | |||
| 27 | STDDEV_GPSVel=0.5; | ||
| 28 | |||
| 29 | STDDEV_ODOVel=0.1; | ||
| 30 | STDDEV_ACCL=1; | ||
| 31 | M_STDDEV_GYRO=0.1; | ||
| 32 | M_STDDEV_POS=0.1; | ||
| 33 | M_STDDEV_VEL=0.02; | ||
| 34 | |||
| 35 | if(strcmp(type,'test')) | ||
| 36 | n = 3; | ||
| 37 | gyroTimeInterval = 0.1; | ||
| 38 | acclTimeInterval = 0.1; | ||
| 39 | M_STDDEV_VEL = 0.2; | ||
| 40 | |||
| 41 | elseif(strcmp(type, 'sim_fast')) | ||
| 42 | n = 3; | ||
| 43 | elseif(strcmp(type, 'sim')) | ||
| 44 | n = 10; | ||
| 45 | elseif(strcmp(type, 'sqcif')) | ||
| 46 | n = 800; | ||
| 47 | elseif(strcmp(type, 'qcif')) | ||
| 48 | n = 500; | ||
| 49 | elseif(strcmp(type, 'vga')) | ||
| 50 | n = 2000; | ||
| 51 | elseif(strcmp(type, 'wuxga')) | ||
| 52 | n = 3000; | ||
| 53 | end | ||
| 54 | |||
| 55 | fprintf(1,'Input size\t\t- (%dx%dx%d)\n', rows, cols,n); | ||
| 56 | pos=zeros(n,3); | ||
| 57 | |||
| 58 | vel=zeros(n,3) + randWrapper(n,3)*STDDEV_ODOVel; | ||
| 59 | pi = 3.1416; | ||
| 60 | |||
| 61 | eul1 = eul2quat([zeros(n,2), randWrapper(n,1)*2*pi]); | ||
| 62 | eul2 = eul2quat([pi, 0, 0]); | ||
| 63 | quat=quatMul(eul1, eul2); | ||
| 64 | |||
| 65 | i=0; | ||
| 66 | index = 0; | ||
| 67 | resultMat = zeros(3,rows); | ||
| 68 | |||
| 69 | while 1 | ||
| 70 | i=i+1; | ||
| 71 | [tStamp, sType, sData, isEOF, index] = readSensorData(index, fid); | ||
| 72 | |||
| 73 | start = photonStartTiming; | ||
| 74 | |||
| 75 | if(sType==2) | ||
| 76 | |||
| 77 | |||
| 78 | %Motion model | ||
| 79 | gyro = sData(1, 1:3); | ||
| 80 | randnWrapper_mat = randnWrapper(n,3); % KVS: We do not have function implementation for randnWrapper() | ||
| 81 | gyro=ones(n,1)*gyro+randnWrapper_mat*M_STDDEV_GYRO; | ||
| 82 | |||
| 83 | abc = gyro.^2; | ||
| 84 | abcd = sumCol(abc); | ||
| 85 | norm_gyro = sqrt(abcd); | ||
| 86 | % norm_gyro=sqrt(sum(gyro.^2,2)); | ||
| 87 | angleAlpha=norm_gyro*gyroTimeInterval; | ||
| 88 | quatDelta=[cos(angleAlpha/2), gyro./(norm_gyro*ones(1,3)+0.00000001).*(sin(angleAlpha/2)*ones(1,3))]; | ||
| 89 | quat=quatMul(quat, quatDelta); | ||
| 90 | |||
| 91 | end | ||
| 92 | |||
| 93 | if(sType==4) | ||
| 94 | %Observation | ||
| 95 | STDDEV_GPSPos=[sData(1,7), 0, 0; 0, sData(1,8), 0; 0, 0, 15]; | ||
| 96 | Opos=sData(1,1:3); | ||
| 97 | |||
| 98 | %Initialize | ||
| 99 | |||
| 100 | randnWrapper_mat = randnWrapper(n,3); | ||
| 101 | if sum(sum(pos))==0 | ||
| 102 | pos=ones(n,1) * Opos + randnWrapper_mat*STDDEV_GPSPos; | ||
| 103 | else | ||
| 104 | rows = size(STDDEV_GPSPos, 1); | ||
| 105 | cols = size(STDDEV_GPSPos, 2); | ||
| 106 | |||
| 107 | temp_STDDEV_GPSPos = ones(rows,cols); | ||
| 108 | for mnrows=1:rows % KVS" Photon rejects this loop becasue of too many nestings ?? | ||
| 109 | for mncols=1:cols | ||
| 110 | temp_STDDEV_GPSPos(mnrows, mncols) = power(STDDEV_GPSPos(mnrows,mncols),-1); | ||
| 111 | end | ||
| 112 | end | ||
| 113 | [temp, w]=mcl(pos, Opos , temp_STDDEV_GPSPos); | ||
| 114 | [quat, vel, pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS); | ||
| 115 | end | ||
| 116 | |||
| 117 | %compare direction | ||
| 118 | Ovel=sData(1,4:6); | ||
| 119 | |||
| 120 | % KVS: We do not have function for norm() yet! So replacing this operating with OvelNorm | ||
| 121 | |||
| 122 | % OvelNorm=norm(Ovel); | ||
| 123 | OvelNorm= 2.0; %1.1169e+09; | ||
| 124 | |||
| 125 | if (OvelNorm>0.5) | ||
| 126 | |||
| 127 | % KVS: Similar here: No impln of norm(), so replacing | ||
| 128 | % norm(Ovel) with OvelNorm value | ||
| 129 | |||
| 130 | Ovel=Ovel/OvelNorm; | ||
| 131 | %trick | ||
| 132 | %quat=addEulNoise(quat, STDDEV_GPSVel); | ||
| 133 | qConj = quatConj(quat); | ||
| 134 | orgWorld=quatRot([1, 0, 0],qConj); | ||
| 135 | eye3 = [1,0,0;0,1,0;0,0,1]; | ||
| 136 | [temp, w]=mcl(orgWorld, Ovel, eye3./STDDEV_GPSVel); | ||
| 137 | [quat, vel, pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS); | ||
| 138 | end | ||
| 139 | |||
| 140 | end | ||
| 141 | |||
| 142 | if(sType==1) | ||
| 143 | |||
| 144 | %Observation | ||
| 145 | Ovel=sData(1,1); | ||
| 146 | [temp, w]=mcl(sqrt(vel(:,1).^2+vel(:,2).^2+vel(:,3).^2), Ovel, 1/(STDDEV_ODOVel)); | ||
| 147 | |||
| 148 | [quat vel pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS); | ||
| 149 | end | ||
| 150 | |||
| 151 | if(sType==3) | ||
| 152 | %Observation | ||
| 153 | accl=sData(1,1:3); | ||
| 154 | |||
| 155 | gtemp = ones(n,1) * [0 0 -9.8]; | ||
| 156 | |||
| 157 | gravity=quatRot(gtemp, quat); | ||
| 158 | eye3 = [1,0,0;0,1,0;0,0,1]; | ||
| 159 | [gravity, w]=mcl(gravity, accl, eye3./(STDDEV_ACCL)); | ||
| 160 | |||
| 161 | [quat, vel, pos]=generateSample(w, quat, vel, pos, M_STDDEV_VEL, M_STDDEV_POS); | ||
| 162 | |||
| 163 | %Motion model | ||
| 164 | accl=ones(n,1)*accl; | ||
| 165 | accl=accl-gravity; | ||
| 166 | pos=pos+quatRot(vel,quatConj(quat))*acclTimeInterval ... | ||
| 167 | +1/2*quatRot(accl,quatConj(quat))*acclTimeInterval^2 ... | ||
| 168 | +randnWrapper(n,3)*M_STDDEV_POS; | ||
| 169 | vel=vel+accl*acclTimeInterval+randnWrapper(n,3)*M_STDDEV_VEL; | ||
| 170 | |||
| 171 | end | ||
| 172 | |||
| 173 | stop = photonEndTiming; | ||
| 174 | |||
| 175 | temp = photonReportTiming(start, stop); | ||
| 176 | elapsed(1) = elapsed(1) + temp(1); | ||
| 177 | elapsed(2) = elapsed(2) + temp(2); | ||
| 178 | |||
| 179 | % Self check | ||
| 180 | |||
| 181 | quatOut = 0; | ||
| 182 | posOut = 0; | ||
| 183 | velOut = 0; | ||
| 184 | |||
| 185 | for ij=1:(size(quat,1)*size(quat,2)) | ||
| 186 | quatOut = quatOut + quat(ij); | ||
