diff options
Diffstat (limited to 'SD-VBS')
6 files changed, 119 insertions, 114 deletions
diff --git a/SD-VBS/benchmarks/localization/src/c/script_localization.c b/SD-VBS/benchmarks/localization/src/c/script_localization.c index ed37132..ddd2215 100644 --- a/SD-VBS/benchmarks/localization/src/c/script_localization.c +++ b/SD-VBS/benchmarks/localization/src/c/script_localization.c | |||
| @@ -78,7 +78,7 @@ int main(int argc, char* argv[]) | |||
| 78 | vel = fSetArray(n, 3, 0); | 78 | vel = fSetArray(n, 3, 0); |
| 79 | ones = fSetArray(n,1,1); | 79 | ones = fSetArray(n,1,1); |
| 80 | 80 | ||
| 81 | 81 | ||
| 82 | F2D *randn; | 82 | F2D *randn; |
| 83 | 83 | ||
| 84 | randn = randWrapper(n,3); | 84 | randn = randWrapper(n,3); |
| @@ -96,38 +96,38 @@ int main(int argc, char* argv[]) | |||
| 96 | } | 96 | } |
| 97 | 97 | ||
| 98 | 98 | ||
| 99 | 99 | ||
| 100 | { | 100 | { |
| 101 | F2D *eulAngle, *randn; | 101 | F2D *eulAngle, *randn; |
| 102 | eulAngle = fSetArray(n, 3, 0); | 102 | eulAngle = fSetArray(n, 3, 0); |
| 103 | randn = randWrapper(n,1); | 103 | randn = randWrapper(n,1); |
| 104 | 104 | ||
| 105 | for(i=0; i<n; i++) | 105 | for(i=0; i<n; i++) |
| 106 | { | 106 | { |
| 107 | subsref(eulAngle, i, 2) = subsref(randn, i, 0) * 2 * pi; | 107 | subsref(eulAngle, i, 2) = subsref(randn, i, 0) * 2 * pi; |
| 108 | } | 108 | } |
| 109 | 109 | ||
| 110 | eul1 = eul2quat(eulAngle); | 110 | eul1 = eul2quat(eulAngle); |
| 111 | fFreeHandle(eulAngle); | 111 | fFreeHandle(eulAngle); |
| 112 | 112 | ||
| 113 | eulAngle = fSetArray(1, 3, 0); | 113 | eulAngle = fSetArray(1, 3, 0); |
| 114 | subsref(eulAngle, 0, 0) = pi; | 114 | subsref(eulAngle, 0, 0) = pi; |
| 115 | eul2 = eul2quat(eulAngle); | 115 | eul2 = eul2quat(eulAngle); |
| 116 | 116 | ||
| 117 | fFreeHandle(randn); | 117 | fFreeHandle(randn); |
| 118 | fFreeHandle(eulAngle); | 118 | fFreeHandle(eulAngle); |
| 119 | } | 119 | } |
| 120 | 120 | ||
| 121 | quat = quatMul(eul1, eul2); | 121 | quat = quatMul(eul1, eul2); |
| 122 | fFreeHandle(eul1); | 122 | fFreeHandle(eul1); |
| 123 | fFreeHandle(eul2); | 123 | fFreeHandle(eul2); |
| 124 | 124 | ||
| 125 | i=0; | 125 | i=0; |
| 126 | index = iSetArray(1,1,-1); | 126 | index = iSetArray(1,1,-1); |
| 127 | sType = iSetArray(1,1,-1); | 127 | sType = iSetArray(1,1,-1); |
| 128 | isEOF = iSetArray(1,1,-1); | 128 | isEOF = iSetArray(1,1,-1); |
| 129 | 129 | ||
| 130 | 130 | ||
| 131 | rows =0; | 131 | rows =0; |
| 132 | cols = 5; | 132 | cols = 5; |
| 133 | STDDEV_GPSPos = fSetArray(3,3,0); | 133 | STDDEV_GPSPos = fSetArray(3,3,0); |
| @@ -136,36 +136,36 @@ int main(int argc, char* argv[]) | |||
| 136 | while(1) | 136 | while(1) |
| 137 | { | 137 | { |
| 138 | icount=icount+1; | 138 | icount=icount+1; |
| 139 | 139 | ||
| 140 | /* | 140 | /* |
| 141 | S = 4 | 141 | S = 4 |
| 142 | A = 3 | 142 | A = 3 |
| 143 | G = 2 | 143 | G = 2 |
| 144 | V = 1 | 144 | V = 1 |
| 145 | */ | 145 | */ |
| 146 | 146 | ||
| 147 | sData = readSensorData(index, fid, sType, isEOF); | 147 | sData = readSensorData(index, fid, sType, isEOF); |
| 148 | rows++; | 148 | rows++; |
| 149 | 149 | ||
| 150 | 150 | ||
| 151 | 151 | ||
| 152 | if( asubsref(sType,0) ==2) | 152 | if( asubsref(sType,0) ==2) |
| 153 | { | 153 | { |
| 154 | 154 | ||
| 155 | //Motion model | 155 | //Motion model |
| 156 | 156 | ||
| 157 | { | 157 | { |
| 158 | int i; | 158 | int i; |
| 159 | F2D *t, *t1; | 159 | F2D *t, *t1; |
| 160 | F2D *abc, *abcd; | 160 | F2D *abc, *abcd; |
| 161 | int qD_r=0, qD_c=0; | 161 | int qD_r=0, qD_c=0; |
| 162 | F2D *cosA, *sinA; | 162 | F2D *cosA, *sinA; |
| 163 | 163 | ||
| 164 | t = fDeepCopyRange(sData, 0, 1, 0, 3); | 164 | t = fDeepCopyRange(sData, 0, 1, 0, 3); |
| 165 | gyro = fMtimes(ones, t); | 165 | gyro = fMtimes(ones, t); |
| 166 | abc = fMallocHandle(gyro->height, gyro->width); | 166 | abc = fMallocHandle(gyro->height, gyro->width); |
| 167 | t1 = fDeepCopy(randW); | 167 | t1 = fDeepCopy(randW); |
| 168 | 168 | ||
| 169 | for(i=0; i<(n*3); i++) | 169 | for(i=0; i<(n*3); i++) |
| 170 | { | 170 | { |
| 171 | asubsref(t1,i) = asubsref(randW,i) * M_STDDEV_GYRO; | 171 | asubsref(t1,i) = asubsref(randW,i) * M_STDDEV_GYRO; |
| @@ -174,103 +174,99 @@ int main(int argc, char* argv[]) | |||
| 174 | } | 174 | } |
| 175 | fFreeHandle(t1); | 175 | fFreeHandle(t1); |
| 176 | abcd = fSum2(abc, 2); | 176 | abcd = fSum2(abc, 2); |
| 177 | 177 | ||
| 178 | norm_gyro = fMallocHandle(abcd->height,abcd->width); | 178 | norm_gyro = fMallocHandle(abcd->height,abcd->width); |
| 179 | angleAlpha = fMallocHandle(abcd->height, abcd->width); | 179 | angleAlpha = fMallocHandle(abcd->height, abcd->width); |
| 180 | 180 | ||
| 181 | for(i=0; i<(abcd->height*abcd->width); i++) | 181 | for(i=0; i<(abcd->height*abcd->width); i++) |
| 182 | { | 182 | { |
| 183 | asubsref(norm_gyro, i) = sqrt(asubsref(abcd,i)); | 183 | asubsref(norm_gyro, i) = sqrt(asubsref(abcd,i)); |
| 184 | asubsref(angleAlpha,i) = asubsref(norm_gyro,i) * gyroTimeInterval; | 184 | asubsref(angleAlpha,i) = asubsref(norm_gyro,i) * gyroTimeInterval; |
| 185 | } | 185 | } |
| 186 | 186 | ||
| 187 | qD_r += angleAlpha->height + gyro->height; | 187 | qD_r += angleAlpha->height + gyro->height; |
| 188 | qD_c += angleAlpha->width + 3; | 188 | qD_c += angleAlpha->width + 3; |
| 189 | 189 | ||
| 190 | fFreeHandle(t); | 190 | fFreeHandle(t); |
| 191 | fFreeHandle(abcd); | 191 | fFreeHandle(abcd); |
| 192 | 192 | ||
| 193 | cosA = fSetArray(angleAlpha->height, angleAlpha->width, 0); | 193 | cosA = fSetArray(angleAlpha->height, angleAlpha->width, 0); |
| 194 | sinA = fSetArray(angleAlpha->height, angleAlpha->width, 0); | 194 | sinA = fSetArray(angleAlpha->height, angleAlpha->width, 0); |
| 195 | 195 | ||
| 196 | for(i=0; i<(cosA->height*cosA->width); i++) | 196 | for(i=0; i<(cosA->height*cosA->width); i++) |
| 197 | asubsref(cosA,i) = cos( asubsref(angleAlpha,i) /2 ); | 197 | asubsref(cosA,i) = cos( asubsref(angleAlpha,i) /2 ); |
| 198 | 198 | ||
| 199 | for(i=0; i<(sinA->height*sinA->width); i++) | 199 | for(i=0; i<(sinA->height*sinA->width); i++) |
| 200 | asubsref(sinA,i) = sin( asubsref(angleAlpha,i) /2 ); | 200 | asubsref(sinA,i) = sin( asubsref(angleAlpha,i) /2 ); |
| 201 | 201 | ||
| 202 | 202 | ||
| 203 | fFreeHandle(abc); | 203 | fFreeHandle(abc); |
| 204 | abc = fSetArray(1,3,1); | 204 | abc = fSetArray(1,3,1); |
| 205 | t1 = fMtimes(norm_gyro, abc); | 205 | t1 = fMtimes(norm_gyro, abc); |
| 206 | t = ffDivide(gyro, t1); | 206 | t = ffDivide(gyro, t1); |
| 207 | fFreeHandle(t1); | 207 | fFreeHandle(t1); |
| 208 | 208 | ||
| 209 | abcd = fMtimes(sinA, abc); | 209 | abcd = fMtimes(sinA, abc); |
| 210 | t1 = fTimes(t, abcd); | 210 | t1 = fTimes(t, abcd); |
| 211 | quatDelta = fHorzcat(cosA, t1); | 211 | quatDelta = fHorzcat(cosA, t1); |
| 212 | 212 | ||
| 213 | fFreeHandle(abcd); | 213 | fFreeHandle(abcd); |
| 214 | fFreeHandle(t); | 214 | fFreeHandle(t); |
| 215 | fFreeHandle(t1); | 215 | fFreeHandle(t1); |
| 216 | fFreeHandle(abc); | 216 | fFreeHandle(abc); |
| 217 | 217 | ||
| 218 | t = quatMul(quat, quatDelta); | 218 | t = quatMul(quat, quatDelta); |
| 219 | fFreeHandle(quat); | 219 | fFreeHandle(quat); |
| 220 | fFreeHandle(quatDelta); | 220 | fFreeHandle(quatDelta); |
| 221 | quat = fDeepCopy(t); | 221 | quat = fDeepCopy(t); |
| 222 | 222 | ||
| 223 | fFreeHandle(t); | 223 | fFreeHandle(t); |
| 224 | fFreeHandle(norm_gyro); | 224 | fFreeHandle(norm_gyro); |
| 225 | fFreeHandle(gyro); | 225 | fFreeHandle(gyro); |
| 226 | fFreeHandle(angleAlpha); | 226 | fFreeHandle(angleAlpha); |
| 227 | fFreeHandle(cosA); | 227 | fFreeHandle(cosA); |
| 228 | fFreeHandle(sinA); | 228 | fFreeHandle(sinA); |
| 229 | 229 | } | |
| 230 | } | ||
| 231 | } | 230 | } |
| 232 | 231 | ||
| 233 | if( asubsref(sType,0) ==4) | 232 | if( asubsref(sType,0) ==4) |
| 234 | { | 233 | { |
| 235 | //Observation | 234 | //Observation |
| 236 | |||
| 237 | float tempSum=0; | 235 | float tempSum=0; |
| 238 | F2D *Ovel; | 236 | F2D *Ovel; |
| 239 | float OvelNorm; | 237 | float OvelNorm; |
| 240 | int i; | 238 | int i; |
| 241 | 239 | ||
| 242 | |||
| 243 | asubsref(STDDEV_GPSPos, 0) = asubsref(sData, 6); | 240 | asubsref(STDDEV_GPSPos, 0) = asubsref(sData, 6); |
| 244 | asubsref(STDDEV_GPSPos, 4) = asubsref(sData,7); | 241 | asubsref(STDDEV_GPSPos, 4) = asubsref(sData,7); |
| 245 | asubsref(STDDEV_GPSPos, 8) = 15; | 242 | asubsref(STDDEV_GPSPos, 8) = 15; |
| 246 | 243 | ||
| 247 | Opos = fDeepCopyRange(sData, 0, 1, 0, 3); | 244 | Opos = fDeepCopyRange(sData, 0, 1, 0, 3); |
| 248 | 245 | ||
| 249 | //Initialize | 246 | //Initialize |
| 250 | |||
| 251 | for(i=0; i<(pos->height*pos->width); i++) | 247 | for(i=0; i<(pos->height*pos->width); i++) |
| 252 | tempSum += asubsref(pos,i); | 248 | tempSum += asubsref(pos,i); |
| 253 | 249 | ||
| 254 | if(tempSum == 0) | 250 | if(tempSum == 0) |
| 255 | { | 251 | { |
| 256 | F2D *t, *t1; | 252 | F2D *t, *t1; |
| 257 | t = fMtimes( randW, STDDEV_GPSPos); | 253 | t = fMtimes( randW, STDDEV_GPSPos); |
| 258 | t1 = fMtimes(ones, Opos); | 254 | t1 = fMtimes(ones, Opos); |
| 259 | 255 | ||
| 260 | for(i=0; i<(pos->height*pos->width); i++) | 256 | for(i=0; i<(pos->height*pos->width); i++) |
| 261 | asubsref(pos,i) = asubsref(t,i) + asubsref(t1,i); | 257 | asubsref(pos,i) = asubsref(t,i) + asubsref(t1,i); |
| 262 | 258 | ||
| 263 | fFreeHandle(t); | 259 | fFreeHandle(t); |
| 264 | fFreeHandle(t1); | 260 | fFreeHandle(t1); |
| 265 | } | 261 | } |
| 266 | else | 262 | else |
| 267 | { | 263 | { |
| 268 | int rows, cols; | 264 | int rows, cols; |
| 269 | int mnrows, mncols; | 265 | int mnrows, mncols; |
| 270 | 266 | ||
| 271 | rows = STDDEV_GPSPos->height; | 267 | rows = STDDEV_GPSPos->height; |
| 272 | cols = STDDEV_GPSPos->width; | 268 | cols = STDDEV_GPSPos->width; |
| 273 | 269 | ||
| 274 | temp_STDDEV_GPSPos = fSetArray(rows,cols,1); | 270 | temp_STDDEV_GPSPos = fSetArray(rows,cols,1); |
| 275 | for( mnrows=0; mnrows<rows; mnrows++) | 271 | for( mnrows=0; mnrows<rows; mnrows++) |
| 276 | { | 272 | { |
| @@ -279,16 +275,16 @@ int main(int argc, char* argv[]) | |||
| 279 | subsref(temp_STDDEV_GPSPos,mnrows,mncols) = pow(subsref(STDDEV_GPSPos,mnrows,mncols),-1); | 275 | subsref(temp_STDDEV_GPSPos,mnrows,mncols) = pow(subsref(STDDEV_GPSPos,mnrows,mncols),-1); |
| 280 | } | 276 | } |
| 281 | } | 277 | } |
| 282 | 278 | ||
| 283 | w = mcl(pos, Opos , temp_STDDEV_GPSPos); | 279 | w = mcl(pos, Opos , temp_STDDEV_GPSPos); |
| 284 | generateSample(w, quat, vel, pos); | 280 | generateSample(w, quat, vel, pos); |
| 285 | } | 281 | } |
| 286 | fFreeHandle(Opos); | 282 | fFreeHandle(Opos); |
| 287 | 283 | ||
| 288 | //compare direction | 284 | //compare direction |
| 289 | Ovel = fDeepCopyRange(sData, 0, 1, 3, 3); | 285 | Ovel = fDeepCopyRange(sData, 0, 1, 3, 3); |
| 290 | OvelNorm=2;//1.1169e+09; | 286 | OvelNorm=2;//1.1169e+09; |
| 291 | 287 | ||
| 292 | if (OvelNorm>0.5) | 288 | if (OvelNorm>0.5) |
| 293 | { | 289 | { |
| 294 | F2D *t; | 290 | F2D *t; |
| @@ -298,7 +294,7 @@ int main(int argc, char* argv[]) | |||
| 298 | Ovel = fDivide(t, OvelNorm); | 294 | Ovel = fDivide(t, OvelNorm); |
| 299 | qConj = quatConj(quat); | 295 | qConj = quatConj(quat); |
| 300 | fFreeHandle(t); | 296 | fFreeHandle(t); |
| 301 | 297 | ||
| 302 | { | 298 | { |
| 303 | t = fSetArray(1,3,0); | 299 | t = fSetArray(1,3,0); |
| 304 | subsref(t,0,0) = 1; | 300 | subsref(t,0,0) = 1; |
| @@ -309,7 +305,7 @@ int main(int argc, char* argv[]) | |||
| 309 | asubsref(t,0) = 1; | 305 | asubsref(t,0) = 1; |
| 310 | asubsref(t,4) = 1; | 306 | asubsref(t,4) = 1; |
| 311 | asubsref(t,8) = 1; | 307 | asubsref(t,8) = 1; |
| 312 | 308 | ||
| 313 | { | 309 | { |
| 314 | int i; | 310 | int i; |
| 315 | for(i=0; i<(t->height*t->width); i++) | 311 | for(i=0; i<(t->height*t->width); i++) |
| @@ -317,7 +313,7 @@ int main(int argc, char* argv[]) | |||
| 317 | w = mcl( orgWorld, Ovel, t); | 313 | w = mcl( orgWorld, Ovel, t); |
| 318 | generateSample(w, quat, vel, pos); | 314 | generateSample(w, quat, vel, pos); |
| 319 | } | 315 | } |
| 320 | 316 | ||
| 321 | fFreeHandle(t); | 317 | fFreeHandle(t); |
| 322 | fFreeHandle(w); | 318 | fFreeHandle(w); |
| 323 | fFreeHandle(orgWorld); | 319 | fFreeHandle(orgWorld); |
| @@ -325,81 +321,81 @@ int main(int argc, char* argv[]) | |||
| 325 | } | 321 | } |
| 326 | fFreeHandle(Ovel); | 322 | fFreeHandle(Ovel); |
| 327 | } | 323 | } |
| 328 | 324 | ||
| 329 | if( asubsref(sType,0) ==1) | 325 | if( asubsref(sType,0) ==1) |
| 330 | { | 326 | { |
| 331 | //Observation | 327 | //Observation |
| 332 | F2D *Ovel; | 328 | F2D *Ovel; |
| 333 | F2D *t, *t1, *t2; | 329 | F2D *t, *t1, *t2; |
| 334 | float valVel; | 330 | float valVel; |
| 335 | 331 | ||
| 336 | t = fSetArray(vel->height, 1, 0); | 332 | t = fSetArray(vel->height, 1, 0); |
| 337 | 333 | ||
| 338 | for(i=0; i<vel->height; i++) | 334 | for(i=0; i<vel->height; i++) |
| 339 | { | 335 | { |
| 340 | subsref(t,i,0) = sqrt(pow(subsref(vel,i,0),2) + pow(subsref(vel,i,1),2) + pow(subsref(vel,i,2),2)); | 336 | subsref(t,i,0) = sqrt(pow(subsref(vel,i,0),2) + pow(subsref(vel,i,1),2) + pow(subsref(vel,i,2),2)); |
| 341 | } | 337 | } |
| 342 | 338 | ||
| 343 | Ovel = fSetArray(1, 1, asubsref(sData,0)); | 339 | Ovel = fSetArray(1, 1, asubsref(sData,0)); |
| 344 | valVel = 1.0/STDDEV_ODOVel; | 340 | valVel = 1.0/STDDEV_ODOVel; |
| 345 | 341 | ||
| 346 | t1 = fSetArray(1,1,(1.0/STDDEV_ODOVel)); | 342 | t1 = fSetArray(1,1,(1.0/STDDEV_ODOVel)); |
| 347 | w = mcl (t, Ovel, t1); | 343 | w = mcl (t, Ovel, t1); |
| 348 | generateSample(w, quat, vel, pos); | 344 | generateSample(w, quat, vel, pos); |
| 349 | 345 | ||
| 350 | 346 | ||
| 351 | fFreeHandle(w); | 347 | fFreeHandle(w); |
| 352 | fFreeHandle(t); | 348 | fFreeHandle(t); |
| 353 | fFreeHandle(t1); | 349 | fFreeHandle(t1); |
| 354 | fFreeHandle(Ovel); | 350 | fFreeHandle(Ovel); |
| 355 | } | 351 | } |
| 356 | 352 | ||
| 357 | if( asubsref(sType,0) ==3) | 353 | if( asubsref(sType,0) ==3) |
| 358 | { | 354 | { |
| 359 | //Observation | 355 | //Observation |
| 360 | F2D *t; | 356 | F2D *t; |
| 361 | t = fSetArray(1, 3, 0); | 357 | t = fSetArray(1, 3, 0); |
| 362 | asubsref(t,2) = -9.8; | 358 | asubsref(t,2) = -9.8; |
| 363 | 359 | ||
| 364 | accl = fDeepCopyRange(sData, 0, 1, 0, 3); | 360 | accl = fDeepCopyRange(sData, 0, 1, 0, 3); |
| 365 | gtemp = fMtimes( ones, t); | 361 | gtemp = fMtimes( ones, t); |
| 366 | gravity = quatRot(gtemp, quat); | 362 | gravity = quatRot(gtemp, quat); |
| 367 | 363 | ||
| 368 | fFreeHandle(gtemp); | 364 | fFreeHandle(gtemp); |
| 369 | fFreeHandle(t); | 365 | fFreeHandle(t); |
| 370 | t = fSetArray(3,3,0); | 366 | t = fSetArray(3,3,0); |
| 371 | asubsref(t,0) = 1; | 367 | asubsref(t,0) = 1; |
| 372 | asubsref(t,4) = 1; | 368 | asubsref(t,4) = 1; |
| 373 | asubsref(t,8) = 1; | 369 | asubsref(t,8) = 1; |
| 374 | 370 | ||
| 375 | { | 371 | { |
| 376 | int i; | 372 | int i; |
| 377 | for(i=0; i<(t->height*t->width); i++) | 373 | for(i=0; i<(t->height*t->width); i++) |
| 378 | asubsref(t,i) = asubsref(t,i)/STDDEV_ACCL; | 374 | asubsref(t,i) = asubsref(t,i)/STDDEV_ACCL; |
| 379 | w = mcl( gravity, accl, t); | 375 | w = mcl( gravity, accl, t); |
| 380 | } | 376 | } |
| 381 | 377 | ||
| 382 | generateSample(w, quat, vel, pos); | 378 | generateSample(w, quat, vel, pos); |
| 383 | fFreeHandle(t); | 379 | fFreeHandle(t); |
| 384 | //Motion model | 380 | //Motion model |
| 385 | t = fMtimes(ones, accl); | 381 | t = fMtimes(ones, accl); |
| 386 | fFreeHandle(accl); | 382 | fFreeHandle(accl); |
| 387 | 383 | ||
| 388 | accl = fMinus(t, gravity); | 384 | accl = fMinus(t, gravity); |
| 389 | 385 | ||
| 390 | fFreeHandle(w); | 386 | fFreeHandle(w); |
| 391 | fFreeHandle(gravity); | 387 | fFreeHandle(gravity); |
| 392 | fFreeHandle(t); | 388 | fFreeHandle(t); |
| 393 | 389 | ||
| 394 | { | 390 | { |
| 395 | //pos=pos+quatRot(vel,quatConj(quat))*acclTimeInterval+1/2*quatRot(accl,quatConj(quat))*acclTimeInterval^2+randn(n,3)*M_STDDEV_POS; | 391 | //pos=pos+quatRot(vel,quatConj(quat))*acclTimeInterval+1/2*quatRot(accl,quatConj(quat))*acclTimeInterval^2+randn(n,3)*M_STDDEV_POS; |
| 396 | 392 | ||
| 397 | F2D *s, *is; | 393 | F2D *s, *is; |
| 398 | int i; | 394 | int i; |
| 399 | is = quatConj(quat); | 395 | is = quatConj(quat); |
| 400 | s = quatRot(vel, is); | 396 | s = quatRot(vel, is); |
| 401 | fFreeHandle(is); | 397 | fFreeHandle(is); |
| 402 | 398 | ||
| 403 | for(i=0; i<(s->height*s->width); i++) | 399 | for(i=0; i<(s->height*s->width); i++) |
| 404 | { | 400 | { |
| 405 | asubsref(s,i) = asubsref(s,i)*acclTimeInterval; //+(1/2); | 401 | asubsref(s,i) = asubsref(s,i)*acclTimeInterval; //+(1/2); |
| @@ -409,9 +405,9 @@ int main(int argc, char* argv[]) | |||
| 409 | pos = fDeepCopy(is); | 405 | pos = fDeepCopy(is); |
| 410 | fFreeHandle(is); | 406 | fFreeHandle(is); |
| 411 | fFreeHandle(s); | 407 | fFreeHandle(s); |
| 412 | 408 | ||
| 413 | /** pos_ above stores: pos+quatRot(vel,quatConj(quat))*acclTimeInterval **/ | 409 | /** pos_ above stores: pos+quatRot(vel,quatConj(quat))*acclTimeInterval **/ |
| 414 | 410 | ||
| 415 | is = quatConj(quat); | 411 | is = quatConj(quat); |
| 416 | s = quatRot(accl, is); | 412 | s = quatRot(accl, is); |
| 417 | t = fDeepCopy(s); | 413 | t = fDeepCopy(s); |
| @@ -419,36 +415,36 @@ int main(int argc, char* argv[]) | |||
| 419 | { | 415 | { |
| 420 | asubsref(t,i) = 1/2*asubsref(s,i)*acclTimeInterval*acclTimeInterval; | 416 | asubsref(t,i) = 1/2*asubsref(s,i)*acclTimeInterval*acclTimeInterval; |
| 421 | } | 417 | } |
| 422 | 418 | ||
| 423 | /** t_ above stores: 1/2*quatRot(accl,quatCong(quat))*acclTimeInterval^2 **/ | 419 | /** t_ above stores: 1/2*quatRot(accl,quatCong(quat))*acclTimeInterval^2 **/ |
| 424 | 420 | ||
| 425 | fFreeHandle(s); | 421 | fFreeHandle(s); |
| 426 | fFreeHandle(is); | 422 | fFreeHandle(is); |
| 427 | s = randnWrapper(n,3); | 423 | s = randnWrapper(n,3); |
| 428 | 424 | ||
| 429 | for(i=0; i<(s->height*s->width); i++) | 425 | for(i=0; i<(s->height*s->width); i++) |
| 430 | { | 426 | { |
| 431 | asubsref(s,i) = asubsref(s,i) * M_STDDEV_POS; | 427 | asubsref(s,i) = asubsref(s,i) * M_STDDEV_POS; |
| 432 | } | 428 | } |
| 433 | 429 | ||
| 434 | /** s_ above stores: randn(n,3)*M_STDDEV_POS **/ | 430 | /** s_ above stores: randn(n,3)*M_STDDEV_POS **/ |
| 435 | 431 | ||
| 436 | is = fPlus(pos, t); | 432 | is = fPlus(pos, t); |
| 437 | fFreeHandle(pos); | 433 | fFreeHandle(pos); |
| 438 | pos = fPlus(is, s); | 434 | pos = fPlus(is, s); |
| 439 | 435 | ||
| 440 | fFreeHandle(s); | 436 | fFreeHandle(s); |
| 441 | fFreeHandle(t); | 437 | fFreeHandle(t); |
| 442 | fFreeHandle(is); | 438 | fFreeHandle(is); |
| 443 | 439 | ||
| 444 | // vel=vel+accl*acclTimeInterval+randn(n,3)*M_STDDEV_VEL; %?? | 440 | // vel=vel+accl*acclTimeInterval+randn(n,3)*M_STDDEV_VEL; %?? |
| 445 | 441 | ||
| 446 | t = fDeepCopy(accl); | 442 | t = fDeepCopy(accl); |
| 447 | for(i=0; i<(accl->height*accl->width); i++) | 443 | for(i=0; i<(accl->height*accl->width); i++) |
| 448 | asubsref(t,i) = asubsref(accl,i) * acclTimeInterval; | 444 | asubsref(t,i) = asubsref(accl,i) * acclTimeInterval; |
| 449 | 445 | ||
| 450 | is = fPlus(vel, t); | 446 | is = fPlus(vel, t); |
| 451 | 447 | ||
| 452 | fFreeHandle(accl); | 448 | fFreeHandle(accl); |
| 453 | fFreeHandle(t); | 449 | fFreeHandle(t); |
| 454 | s = randnWrapper(n,3); | 450 | s = randnWrapper(n,3); |
| @@ -456,30 +452,30 @@ int main(int argc, char* argv[]) | |||
| 456 | { | 452 | { |
| 457 | asubsref(s,i) = asubsref(s,i) * M_STDDEV_VEL; | 453 | asubsref(s,i) = asubsref(s,i) * M_STDDEV_VEL; |
| 458 | } | 454 | } |
| 459 | 455 | ||
| 460 | fFreeHandle(vel); | 456 | fFreeHandle(vel); |
| 461 | vel = fPlus(is, s); | 457 | vel = fPlus(is, s); |
| 462 | fFreeHandle(is); | 458 | fFreeHandle(is); |
| 463 | fFreeHandle(s); | 459 | fFreeHandle(s); |
| 464 | } | 460 | } |
| 465 | 461 | ||
| 466 | } | 462 | } |
| 467 | 463 | ||
| 468 | // Self check | 464 | // Self check |
| 469 | { | 465 | { |
| 470 | F2D* temp; | 466 | F2D* temp; |
| 471 | float quatOut=0, velOut=0, posOut=0; | 467 | float quatOut=0, velOut=0, posOut=0; |
| 472 | int i; | 468 | int i; |
| 473 | 469 | ||
| 474 | for(i=0; i<(quat->height*quat->width); i++) | 470 | for(i=0; i<(quat->height*quat->width); i++) |
| 475 | quatOut += asubsref(quat, i); | 471 | quatOut += asubsref(quat, i); |
| 476 | 472 | ||
| 477 | for(i=0; i<(vel->height*vel->width); i++) | 473 | for(i=0; i<(vel->height*vel->width); i++) |
| 478 | velOut += asubsref(vel, i); | 474 | velOut += asubsref(vel, i); |
| 479 | 475 | ||
| 480 | for(i=0; i<(pos->height*pos->width); i++) | 476 | for(i=0; i<(pos->height*pos->width); i++) |
| 481 | posOut += asubsref(pos, i); | 477 | posOut += asubsref(pos, i); |
| 482 | 478 | ||
| 483 | subsref(resultMat,0,icount) = quatOut; | 479 | subsref(resultMat,0,icount) = quatOut; |
| 484 | subsref(resultMat,1,icount) = velOut; | 480 | subsref(resultMat,1,icount) = velOut; |
| 485 | subsref(resultMat,2,icount) = posOut; | 481 | subsref(resultMat,2,icount) = posOut; |
| @@ -490,10 +486,16 @@ int main(int argc, char* argv[]) | |||
| 490 | if (asubsref(isEOF,0) == 1) | 486 | if (asubsref(isEOF,0) == 1) |
| 491 | break; | 487 | break; |
| 492 | } | 488 | } |
| 489 | fFreeHandle(STDDEV_GPSPos); | ||
| 490 | iFreeHandle(index); | ||
| 491 | iFreeHandle(sType); | ||
| 492 | iFreeHandle(isEOF); | ||
| 493 | fFreeHandle(quat); | ||
| 494 | fFreeHandle(randW); | ||
| 493 | } | 495 | } |
| 494 | 496 | ||
| 495 | #ifdef CHECK | 497 | #ifdef CHECK |
| 496 | 498 | ||
| 497 | // Self checking - use expected.txt from data directory | 499 | // Self checking - use expected.txt from data directory |
| 498 | { | 500 | { |
| 499 | int ret=0; | 501 | int ret=0; |
| @@ -508,18 +510,11 @@ int main(int argc, char* argv[]) | |||
| 508 | // Self checking done | 510 | // Self checking done |
| 509 | #endif | 511 | #endif |
| 510 | 512 | ||
| 511 | |||
| 512 | fFreeHandle(STDDEV_GPSPos); | ||
| 513 | iFreeHandle(index); | ||
| 514 | iFreeHandle(sType); | ||
| 515 | iFreeHandle(isEOF); | ||
| 516 | fFreeHandle(fid); | 513 | fFreeHandle(fid); |
| 517 | fFreeHandle(resultMat); | 514 | fFreeHandle(resultMat); |
| 518 | fFreeHandle(pos); | 515 | fFreeHandle(pos); |
| 519 | fFreeHandle(vel); | 516 | fFreeHandle(vel); |
| 520 | fFreeHandle(quat); | ||
| 521 | fFreeHandle(ones); | 517 | fFreeHandle(ones); |
| 522 | fFreeHandle(randW); | ||
| 523 | fFreeHandle(randn); | 518 | fFreeHandle(randn); |
| 524 | WRITE_TO_FILE | 519 | WRITE_TO_FILE |
| 525 | return 0; | 520 | return 0; |
diff --git a/SD-VBS/benchmarks/multi_ncut/src/c/script_multi_ncut.c b/SD-VBS/benchmarks/multi_ncut/src/c/script_multi_ncut.c index 77c6b71..b836824 100644 --- a/SD-VBS/benchmarks/multi_ncut/src/c/script_multi_ncut.c +++ b/SD-VBS/benchmarks/multi_ncut/src/c/script_multi_ncut.c | |||
| @@ -75,6 +75,7 @@ int main(int argc, char* argv[]) | |||
| 75 | free(segments); | 75 | free(segments); |
| 76 | free(tempOut); | 76 | free(tempOut); |
| 77 | free(imageOut); | 77 | free(imageOut); |
| 78 | iFreeHandle(kernel); | ||
| 78 | free(edgeWeights); | 79 | free(edgeWeights); |
| 79 | free(in); | 80 | free(in); |
| 80 | free(ind); | 81 | free(ind); |
diff --git a/SD-VBS/benchmarks/sift/src/c/script_sift.c b/SD-VBS/benchmarks/sift/src/c/script_sift.c index ab39ae3..40b16d4 100644 --- a/SD-VBS/benchmarks/sift/src/c/script_sift.c +++ b/SD-VBS/benchmarks/sift/src/c/script_sift.c | |||
| @@ -7,7 +7,7 @@ Author: Sravanthi Kota Venkata | |||
| 7 | #include "sift.h" | 7 | #include "sift.h" |
| 8 | #include <malloc.h> | 8 | #include <malloc.h> |
| 9 | #include "extra.h" | 9 | #include "extra.h" |
| 10 | #define SIFT_MEM 1<<29 | 10 | #define SIFT_MEM 1<<24 |
| 11 | void normalizeImage(F2D* image) | 11 | void normalizeImage(F2D* image) |
| 12 | { | 12 | { |
| 13 | int i; | 13 | int i; |
diff --git a/SD-VBS/benchmarks/svm/src/c/script_svm.c b/SD-VBS/benchmarks/svm/src/c/script_svm.c index f1ce963..d77291c 100644 --- a/SD-VBS/benchmarks/svm/src/c/script_svm.c +++ b/SD-VBS/benchmarks/svm/src/c/script_svm.c | |||
| @@ -16,7 +16,7 @@ int main(int argc, char* argv[]) | |||
| 16 | int iter, N, Ntst, i, j, k, n; | 16 | int iter, N, Ntst, i, j, k, n; |
| 17 | F2D* trn1, *tst1, *trn2, *tst2, *Yoffset; | 17 | F2D* trn1, *tst1, *trn2, *tst2, *Yoffset; |
| 18 | alphaRet* alpha; | 18 | alphaRet* alpha; |
| 19 | F2D *a_result, *result; | 19 | F2D *a_result, *result = NULL; |
| 20 | F2D *s; | 20 | F2D *s; |
| 21 | F2D *b_result; | 21 | F2D *b_result; |
| 22 | F2D *Xtst, *Ytst; | 22 | F2D *Xtst, *Ytst; |
| @@ -87,6 +87,10 @@ int main(int argc, char* argv[]) | |||
| 87 | tst2 = readFile(im1); | 87 | tst2 = readFile(im1); |
| 88 | 88 | ||
| 89 | for_each_job { | 89 | for_each_job { |
| 90 | // Clear the results from our last job | ||
| 91 | if (result) | ||
| 92 | fFreeHandle(result); | ||
| 93 | |||
| 90 | alpha = getAlphaFromTrainSet(N, trn1, trn2, iter); | 94 | alpha = getAlphaFromTrainSet(N, trn1, trn2, iter); |
| 91 | a_result = alpha->a_result; | 95 | a_result = alpha->a_result; |
| 92 | b_result = alpha->b_result; | 96 | b_result = alpha->b_result; |
| @@ -133,6 +137,14 @@ int main(int argc, char* argv[]) | |||
| 133 | fFreeHandle(s); | 137 | fFreeHandle(s); |
| 134 | asubsref(result,n) = maxs; | 138 | asubsref(result,n) = maxs; |
| 135 | } | 139 | } |
| 140 | |||
| 141 | fFreeHandle(Yoffset); | ||
| 142 | fFreeHandle(alpha->a_result); | ||
| 143 | fFreeHandle(alpha->b_result); | ||
| 144 | fFreeHandle(alpha->X); | ||
| 145 | free(alpha); | ||
| 146 | fFreeHandle(Xtst); | ||
| 147 | fFreeHandle(Ytst); | ||
| 136 | } | 148 | } |
| 137 | 149 | ||
| 138 | #ifdef CHECK | 150 | #ifdef CHECK |
| @@ -154,14 +166,7 @@ int main(int argc, char* argv[]) | |||
| 154 | fFreeHandle(tst1); | 166 | fFreeHandle(tst1); |
| 155 | fFreeHandle(trn2); | 167 | fFreeHandle(trn2); |
| 156 | fFreeHandle(tst2); | 168 | fFreeHandle(tst2); |
| 157 | fFreeHandle(Yoffset); | ||
| 158 | fFreeHandle(result); | 169 | fFreeHandle(result); |
| 159 | fFreeHandle(alpha->a_result); | ||
| 160 | fFreeHandle(alpha->b_result); | ||
| 161 | fFreeHandle(alpha->X); | ||
| 162 | free(alpha); | ||
| 163 | fFreeHandle(Xtst); | ||
| 164 | fFreeHandle(Ytst); | ||
| 165 | WRITE_TO_FILE | 170 | WRITE_TO_FILE |
| 166 | return 0; | 171 | return 0; |
| 167 | } | 172 | } |
diff --git a/SD-VBS/benchmarks/texture_synthesis/src/c/script_texture_synthesis.c b/SD-VBS/benchmarks/texture_synthesis/src/c/script_texture_synthesis.c index 2ebb423..a3042e5 100644 --- a/SD-VBS/benchmarks/texture_synthesis/src/c/script_texture_synthesis.c +++ b/SD-VBS/benchmarks/texture_synthesis/src/c/script_texture_synthesis.c | |||
| @@ -5,7 +5,7 @@ Author: Sravanthi Kota Venkata | |||
| 5 | #include "texture.h" | 5 | #include "texture.h" |
| 6 | #include <malloc.h> | 6 | #include <malloc.h> |
| 7 | #include "extra.h" | 7 | #include "extra.h" |
| 8 | #define TEXTURE_MEM 1<<28 | 8 | #define TEXTURE_MEM 1<<20 |
| 9 | int WIDTHin,HEIGHTin; | 9 | int WIDTHin,HEIGHTin; |
| 10 | F2D *target, *result; | 10 | F2D *target, *result; |
| 11 | int WIDTH,HEIGHT; | 11 | int WIDTH,HEIGHT; |
| @@ -48,6 +48,8 @@ int main(int argc, char **argv) | |||
| 48 | fFreeHandle(target); | 48 | fFreeHandle(target); |
| 49 | fFreeHandle(result); | 49 | fFreeHandle(result); |
| 50 | free(atlas); | 50 | free(atlas); |
| 51 | free(xloopout - WIDTH/2); | ||
| 52 | free(yloopout - HEIGHT/2); | ||
| 51 | WRITE_TO_FILE | 53 | WRITE_TO_FILE |
| 52 | return 0; | 54 | return 0; |
| 53 | } | 55 | } |
diff --git a/SD-VBS/benchmarks/texture_synthesis/src/c/texture.c b/SD-VBS/benchmarks/texture_synthesis/src/c/texture.c index fb0f1ae..b268df9 100644 --- a/SD-VBS/benchmarks/texture_synthesis/src/c/texture.c +++ b/SD-VBS/benchmarks/texture_synthesis/src/c/texture.c | |||
| @@ -150,6 +150,8 @@ void create_texture(F2D *image, params *data) | |||
| 150 | arrayref(atlas,aa(j,i)+1) = besty; | 150 | arrayref(atlas,aa(j,i)+1) = besty; |
| 151 | } | 151 | } |
| 152 | } | 152 | } |
| 153 | free(candlistx); | ||
| 154 | free(candlisty); | ||
| 153 | } | 155 | } |
| 154 | 156 | ||
| 155 | // Creates a list of valid candidates for given pixel using only L-shaped causal area | 157 | // Creates a list of valid candidates for given pixel using only L-shaped causal area |
