From 0efc775370d2ff91927d1b383a99eab78dc5538f Mon Sep 17 00:00:00 2001 From: leochanj Date: Wed, 21 Oct 2020 01:52:54 -0400 Subject: debug libextra and remove matlab FLUSH_CACHES --- .../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 -- 20 files changed, 611 deletions(-) delete mode 100644 SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m delete mode 100644 SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m delete mode 100644 SD-VBS/benchmarks/localization/src/matlab/drawLog.m delete mode 100644 SD-VBS/benchmarks/localization/src/matlab/eul2quat.m delete mode 100644 SD-VBS/benchmarks/localization/src/matlab/generateSample.m delete mode 100644 SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m delete mode 100644 SD-VBS/benchmarks/localization/src/matlab/getGroundData.m delete mode 100644 SD-VBS/benchmarks/localization/src/matlab/mcl.m delete mode 100644 SD-VBS/benchmarks/localization/src/matlab/mclWhole.m delete mode 100644 SD-VBS/benchmarks/localization/src/matlab/quat2eul.m delete mode 100644 SD-VBS/benchmarks/localization/src/matlab/quatConj.m delete mode 100644 SD-VBS/benchmarks/localization/src/matlab/quatMul.m delete mode 100644 SD-VBS/benchmarks/localization/src/matlab/quatRot.m delete mode 100644 SD-VBS/benchmarks/localization/src/matlab/readLoc.m delete mode 100644 SD-VBS/benchmarks/localization/src/matlab/readMatrix.m delete mode 100644 SD-VBS/benchmarks/localization/src/matlab/readSensorData.m delete mode 100644 SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m delete mode 100644 SD-VBS/benchmarks/localization/src/matlab/selfCheck.m delete mode 100644 SD-VBS/benchmarks/localization/src/matlab/sumCol.m delete 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/matlab/addEulNoise.m b/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m deleted file mode 100644 index 3d2ac76..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m +++ /dev/null @@ -1,12 +0,0 @@ -function retQuat=addEulNoise(quat, STD) -n=size(quat,1); - -% KVS: replacing randnWrapper(n,3) with rand(n,3) -gyro=randnWrapper(n,3)*STD; - -norm_gyro=sqrt(sum(gyro.^2,2)); -angleAlpha=norm_gyro; -quatDelta=[cos(angleAlpha/2), gyro./(norm_gyro*ones(1,3)).*(sin(angleAlpha/2)*ones(1,3))]; - -retQuat=quatMul(quat, quatDelta); - diff --git a/SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m b/SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m deleted file mode 100644 index 060713a..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/calculate3DGaussian.m +++ /dev/null @@ -1,11 +0,0 @@ -function [meanColor A]=calculate3DGaussian(data) -n_data=size(data,1); -n_channel=size(data,2); -meanColor=mean(data); -diff=double(data)-ones(n_data,1)*meanColor; -diifTr = transpose(diff); -Ainv=(diffTr*diff/n_data); -AinvTr = transpose(Ainv); -A=inv(AinvTr); - - diff --git a/SD-VBS/benchmarks/localization/src/matlab/drawLog.m b/SD-VBS/benchmarks/localization/src/matlab/drawLog.m deleted file mode 100644 index f88e078..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/drawLog.m +++ /dev/null @@ -1,44 +0,0 @@ -function drawLog(data, ground_data) -subplot(1,3,1); -[nr nc]=size(data); -for i=1:nr - x=data(i,2); - y=data(i,3); - z=data(i,4); - R=[1 0 0; 0 cos(x) sin(x); 0 -sin(x) cos(x)]... - *[cos(y) 0 -sin(y); 0 1 0; sin(y) 0 cos(y)]... - *[cos(z) sin(z) 0; -sin(z) cos(z) 0; 0 0 1]; - coord=R*[1 0 0]'; - axis([-1 1 -1 1 -1 1]);axis on - plot3([0 coord(1)], [0 coord(2)], [0 coord(3)], 'b'); hold on -end - -[nr nc]=size(ground_data); -for i=1:nr -x=ground_data(i,2); -y=ground_data(i,3); -z=ground_data(i,4); -R=[1 0 0; 0 cos(x) sin(x); 0 -sin(x) cos(x)]... - *[cos(y) 0 -sin(y); 0 1 0; sin(y) 0 cos(y)]... - *[cos(z) sin(z) 0; -sin(z) cos(z) 0; 0 0 1]; -coord=R*[1 0 0]'; -coord2=R*[0 -1 0]'; -axis([-1 1 -1 1 -1 1]);axis on -plot3([0 coord(1)], [0 coord(2)], [0 coord(3)], 'r'); hold on -plot3([0 coord2(1)], [0 coord2(2)], [0 coord2(3)], 'g'); -end - -xlabel('x'); -ylabel('y'); -zlabel('z'); -hold off -drawnow -subplot(1,3,2); -Xoffset=4422610; -Yoffset=483660; -axis([4422610-Xoffset 4422660-Xoffset 483620-Yoffset 483720-Yoffset]);axis on -scatter(data(:,5)-Xoffset, data(:,6)-Yoffset, 8, 'b'); hold on -axis([4422610-Xoffset 4422660-Xoffset 483620-Yoffset 483720-Yoffset]);axis on -scatter(ground_data(:,5)-Xoffset, ground_data(:,6)-Yoffset, 10, 'r'); hold off -drawnow - diff --git a/SD-VBS/benchmarks/localization/src/matlab/eul2quat.m b/SD-VBS/benchmarks/localization/src/matlab/eul2quat.m deleted file mode 100644 index 57001a8..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/eul2quat.m +++ /dev/null @@ -1,9 +0,0 @@ -function retQuat=eul2quat(eulAngle) -x=eulAngle(:,1); -y=eulAngle(:,2); -z=eulAngle(:,3); -retQuat=[cos(x/2).*cos(y/2).*cos(z/2)+sin(x/2).*sin(y/2).*sin(z/2) ... - sin(x/2).*cos(y/2).*cos(z/2)-cos(x/2).*sin(y/2).*sin(z/2) ... - cos(x/2).*sin(y/2).*cos(z/2)+sin(x/2).*cos(y/2).*sin(z/2) ... - cos(x/2).*cos(y/2).*sin(z/2)-sin(x/2).*sin(y/2).*cos(z/2)]; - diff --git a/SD-VBS/benchmarks/localization/src/matlab/generateSample.m b/SD-VBS/benchmarks/localization/src/matlab/generateSample.m deleted file mode 100644 index f3658f8..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/generateSample.m +++ /dev/null @@ -1,25 +0,0 @@ -function [retQuat retVel retPos]=generateSample(w, quat, vel, pos, STDDEV_VEL, STDDEV_POS) -sampledXId=weightedSample(w); - -rows = size(sampledXId, 1); -cols = size(sampledXId, 2); - -if(cols > 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 deleted file mode 100644 index 6a54530..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/get3DGaussianProb.m +++ /dev/null @@ -1,22 +0,0 @@ -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 deleted file mode 100644 index 7cf5f9c..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/getGroundData.m +++ /dev/null @@ -1,5 +0,0 @@ -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 deleted file mode 100644 index 8ab8aa6..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/mcl.m +++ /dev/null @@ -1,9 +0,0 @@ -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 deleted file mode 100644 index 4e78b8f..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m +++ /dev/null @@ -1,18 +0,0 @@ -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 deleted file mode 100644 index a6b48ee..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/quat2eul.m +++ /dev/null @@ -1,6 +0,0 @@ -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 deleted file mode 100644 index ff494b4..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/quatConj.m +++ /dev/null @@ -1,5 +0,0 @@ -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 deleted file mode 100644 index b397554..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/quatMul.m +++ /dev/null @@ -1,18 +0,0 @@ -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 deleted file mode 100644 index 3688172..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/quatRot.m +++ /dev/null @@ -1,24 +0,0 @@ -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 deleted file mode 100644 index fc679d4..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/readLoc.m +++ /dev/null @@ -1,53 +0,0 @@ -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 deleted file mode 100644 index fe684c5..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/readMatrix.m +++ /dev/null @@ -1,28 +0,0 @@ -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 deleted file mode 100644 index e84a6db..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/readSensorData.m +++ /dev/null @@ -1,46 +0,0 @@ -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 deleted file mode 100644 index 44ce3aa..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/script_run_profile.m +++ /dev/null @@ -1,209 +0,0 @@ -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 deleted file mode 100644 index ac80ebd..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/selfCheck.m +++ /dev/null @@ -1,33 +0,0 @@ -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 deleted file mode 100644 index ad0114e..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/sumCol.m +++ /dev/null @@ -1,14 +0,0 @@ -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 deleted file mode 100644 index dc9274b..0000000 --- a/SD-VBS/benchmarks/localization/src/matlab/weightedSample.m +++ /dev/null @@ -1,20 +0,0 @@ -%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