diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..4e63be5 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +isam/build diff --git a/Processer.m b/Processer.m index 155f5a5..db949d9 100644 --- a/Processer.m +++ b/Processer.m @@ -22,6 +22,8 @@ obj.odoms.pos(:,i)=pos; obj.odoms.theta(i)=theta; end + % ID1 basically has all the objects associated uniquely and they + % are being assigned here to the obj_id field obj.measurements.obj_id = 1:length(lm_edge.id1); for i=1:length(lm_edge.id1) obj.objects(i).id=i; @@ -112,7 +114,7 @@ function plot(obj) plot(odom(1,:),odom(2,:),'k'); hold on; - label={'tajectory'}; + label={'trajectory'}; p=[obj.objects.P]; objpose = [obj.objects.pos]; for i=1:size(p,1) @@ -262,6 +264,8 @@ function plot(obj) obj=obj.DPsample(); obj.write_isam('isam_input.txt'); !isam/bin/isam isam_input.txt -W isam_output.txt -B + % Add an exception where the code breaks when output isnt + % genereated because of isam failure. [obj] = obj.read_isam('isam_output.txt'); end end diff --git a/Processer.m~ b/Processer.m~ new file mode 100644 index 0000000..db949d9 --- /dev/null +++ b/Processer.m~ @@ -0,0 +1,355 @@ +classdef Processer +properties + odoms + objects + measurements + fp_thre=0 + DP_alpha=0.9 + odom_noise = [0.1; 0.1; 0.1] + info_odom = [10 0 0 10 0 10] + lm_noise = [0.5; 0.5; 1000] + info_lm = [2 0 2] +end +methods + function obj = setupobjects(obj,node_edge, lm_edge) + obj.measurements=lm_edge; + obj.odoms=node_edge; + theta=0; pos = zeros(2,1); + for i=1:length(node_edge.dtheta) + R=[cos(theta) sin(theta); -sin(theta) cos(theta)]; + pos=pos+R*obj.odoms.dpos(:,i); + theta=theta + obj.odoms.dtheta(i); theta = mod(theta+pi,2*pi)-pi; + obj.odoms.pos(:,i)=pos; + obj.odoms.theta(i)=theta; + end + % ID1 basically has all the objects associated uniquely and they + % are being assigned here to the obj_id field + obj.measurements.obj_id = 1:length(lm_edge.id1); + for i=1:length(lm_edge.id1) + obj.objects(i).id=i; + obj.objects(i).P=zeros(5,1); + obj.objects(i).P(lm_edge.label(i))=1; + obj.objects(i).msts = i; + t = obj.odoms.theta(lm_edge.id1(i)); + R=[cos(t) sin(t); -sin(t) cos(t)]; + obj.objects(i).pos = R*lm_edge.dpos(:,i)+obj.odoms.pos(:,lm_edge.id1(i)); + end + end + + function [] = write_isam(obj, filename) + %% write file + fid = fopen(filename,'w'); + + for i=1:length(obj.odoms.dtheta) + fprintf(fid,'ODOMETRY %d %d %f %f %f',i-1,i,obj.odoms.dpos(1,i),obj.odoms.dpos(2,i),-obj.odoms.dtheta(i)); + for j=1:length(obj.info_odom) fprintf(fid,' %f',obj.info_odom(j)); end + fprintf(fid,'\n'); + end + + for k=1:length(obj.objects) + if( length(obj.objects(k).msts)>obj.fp_thre) + for i=obj.objects(k).msts + fprintf(fid,'LANDMARK %d %d %f %f', obj.measurements.id1(i), obj.objects(k).id+1000,... + obj.measurements.dpos(1,i), obj.measurements.dpos(2,i)); + for j=1:length(obj.info_lm) fprintf(fid,' %f',obj.info_lm(j)); end + fprintf(fid,'\n'); + end + end + end + fclose(fid); + end + + function [obj, Iodom, Iobj] = read_isam(obj, filename) + fid = fopen(filename); + tline = fgetl(fid); + odom = []; + objects=[]; + entropy=[]; + while ischar(tline) + if strcmp(tline(1:11),'Pose2d_Node') + line = tline(13:end); + line(line=='(')=[]; line(line==')')=[];line(line==';')=','; + odom = [odom; str2num(line)]; + end + if strcmp(tline(1:12),'Point2d_Node') + line = tline(14:end); + line(line=='(')=[]; line(line==')')=[]; + objects = [objects; str2num(line)]; + end + if strcmp(tline(1:7),'entropy') + line = tline(9:end); + entropy = [entropy; str2num(line)]; + end + tline = fgetl(fid); + end + fclose(fid); + + N=size(odom,1); + obj.odoms.pos = odom(:,2:3)'; + obj.odoms.theta = -odom(:,4)'; +% obj.odoms.entropy = entropy(1:N,:); + + % id = [obj.objects.id]; +% id = unique(id,'stable'); + id=[]; + for i=1:length(obj.objects) + if( length(obj.objects(i).msts)>obj.fp_thre) + id=[id obj.objects(i).id]; + end + end + for i=1:length(objects) + obj_idx = [obj.objects.id]==id(i); + obj.objects(obj_idx).pos = objects(i,2:end)'; +% obj.objects(obj_idx).entropy = entropy(i,2); + end + end + + function plot(obj) + odom = obj.odoms.pos; + fig = figure; + set(fig,'Position', [100, 100, 400, 300]); + set(fig,'Units','Inches'); + pos = get(fig,'Position'); + set(fig,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)]) + plot(odom(1,:),odom(2,:),'k'); + hold on; + + label={'trajectory'}; + p=[obj.objects.P]; + objpose = [obj.objects.pos]; + for i=1:size(p,1) + idx = p(i,:)>obj.fp_thre; + if sum(idx)>0 + label{end+1}=['class ' num2str(i)]; + switch i + case 1 + plot(objpose(1,idx),objpose(2,idx),'bo','MarkerFaceColor','b'); + case 2 + plot(objpose(1,idx),objpose(2,idx),'rd','MarkerFaceColor','r'); + case 3 + plot(objpose(1,idx),objpose(2,idx),'ms','MarkerFaceColor','m'); + case 4 + plot(objpose(1,idx),objpose(2,idx),'g^','MarkerFaceColor',[0.2 1 0.2]); + case 5 + plot(objpose(1,idx),objpose(2,idx),'yp','MarkerFaceColor',[1 0.7 0.3],... + 'MarkerSize',15); + end + end + end + axis equal; axis off; + legend(label); + end + + function obj = RobustSlam(obj) + for k=1:length(obj.measurements.id1) + t = obj.odoms.theta(obj.measurements.id1(k)); + R = [cos(t) sin(t); -sin(t) cos(t)]; + pos(:,k) = obj.odoms.pos(:,obj.measurements.id1(k))+R*obj.measurements.dpos(:,k); + end + + for i=1:5 + objects(i).id = i; + objects(i).P = zeros(5,1); + objects(i).P(i)=1; + objects(i).msts = find(obj.measurements.label==i,1,'first'); + obj.measurements.obj_id(objects(i).msts)=i; + end + for k=1:length(obj.measurements.id1); + msts = [objects.msts]; + sq_dist = sum((pos(:,msts) - repmat(pos(:,k),1,length(msts))).^2,1); + posterior = exp(-4*sq_dist).*(obj.measurements.label(msts)==obj.measurements.label(k)); + [prob, I]=max(posterior); + I = obj.measurements.obj_id(msts(I)); + l = obj.measurements.label(k); + if(prob<1) + if (prob>obj.DP_alpha) + objects(I).P(l)=objects(I).P(l)+1; + objects(I).msts = [objects(I).msts k]; + obj.measurements.obj_id(k)=objects(I).id; + else + obj.measurements.obj_id(k)=0; + end + end + end + for i=1:length(objects) + objects(i).pos = mean(pos(:,objects(i).msts),2); + end + obj.objects=objects; + end + + function obj = OpenLoop(obj) + for k=1:length(obj.measurements.id1) + t = obj.odoms.theta(obj.measurements.id1(k)); + R = [cos(t) sin(t); -sin(t) cos(t)]; + pos(:,k) = obj.odoms.pos(:,obj.measurements.id1(k))+R*obj.measurements.dpos(:,k); + end + + objects.id = 1; + objects.P = zeros(5,1); objects.P(obj.measurements.label(1))=1; +% objects.pos = pos(:,1); + objects.msts = 1; + obj.measurements.obj_id(1)=1; + for k=2:length(obj.measurements.id1); +% p = [objects.P]; +% obj_poses = [objects.pos]; +% N = length(objects); + + sq_dist = sum((pos(:,1:k-1) - repmat(pos(:,k),1,k-1)).^2,1); +% dp_prior = p(obj.measurements.label(k),:); + posterior = exp(-4*sq_dist).*(obj.measurements.label(1:k-1)==obj.measurements.label(k)); + [prob, I]=max(posterior); + I = obj.measurements.obj_id(I); + l = obj.measurements.label(k); + if(prob>obj.DP_alpha) + objects(I).P(l)=objects(I).P(l)+1; + objects(I).msts = [objects(I).msts k]; + obj.measurements.obj_id(k)=objects(I).id; +% objects(I).pos = mean(pos(:,objects(I).msts),2); + else + N_obj = max([objects.id]); + object.id = N_obj+1; + object.P = zeros(5,1);object.P(l)=1; +% object.pos = pos(:,k); + object.msts = k; + objects(end+1)=object; + obj.measurements.obj_id(k) = N_obj+1; + end + end + for i=1:length(objects) + objects(i).pos = mean(pos(:,objects(i).msts),2); + end + obj.objects=objects; + end + + function obj = DPsample(obj) + for k=length(obj.measurements.id1):-1:1 + obj_idx = find([obj.objects.id]==obj.measurements.obj_id(k)); + l=obj.measurements.label(k); + obj.objects(obj_idx).P(l)=obj.objects(obj_idx).P(l)-1; + if sum(obj.objects(obj_idx).P)==0 + obj.objects(obj_idx)=[]; + else + obj.objects(obj_idx).msts(obj.objects(obj_idx).msts==k)=[]; + end + p = [obj.objects.P]; + obj_poses = [obj.objects.pos]; + N = length(obj.objects); + + t = obj.odoms.theta(obj.measurements.id1(k)); + R = [cos(t) sin(t); -sin(t) cos(t)]; + pos = obj.odoms.pos(:,obj.measurements.id1(k))+R*obj.measurements.dpos(:,k); + + dp_prior = p(obj.measurements.label(k),:); + sq_dist = sum((obj_poses - repmat(pos,1,N)).^2,1); + posterior = exp(-4*sq_dist).*dp_prior; + [prob, I]=max(posterior); + if(prob>obj.DP_alpha) + obj.objects(I).P(l)=obj.objects(I).P(l)+1; + obj.objects(I).msts = [obj.objects(I).msts k]; + obj.measurements.obj_id(k)=obj.objects(I).id; + else + N_obj = max([obj.objects.id]); + object.id = N_obj+1; + object.P = zeros(5,1);object.P(l)=1; + object.pos = pos; + object.msts = k; + obj.objects=[obj.objects object]; + obj.measurements.obj_id(k) = N_obj+1; + end + end + end + + function [obj]= optimize(obj,maxiteration) + for i=1:maxiteration + obj=obj.DPsample(); + obj.write_isam('isam_input.txt'); + !isam/bin/isam isam_input.txt -W isam_output.txt -B + % Add an exception where the code breaks when output isnt + % genereated because of isam failure. + [obj] = obj.read_isam('isam_output.txt'); + end + end + + function [Iodom, Iobj] = computeEntropy(obj) + factorgraph = gtsam.NonlinearFactorGraph; + estimates = gtsam.Values; + odom_cov = gtsam.noiseModel.Diagonal.Sigmas(obj.odom_noise); + ms_cov = gtsam.noiseModel.Diagonal.Sigmas(obj.lm_noise); + + factorgraph.add(gtsam.NonlinearEqualityPose2(0,gtsam.Pose2)); + estimates.insert(0, gtsam.Pose2); + + odoms = [obj.odoms.pos; obj.odoms.theta]; + dodom = [obj.odoms.dpos; obj.odoms.dtheta]; + for k=1:size(dodom,2) + estimates.insert(k,gtsam.Pose2(odoms(:,k))); + factorgraph.add(gtsam.BetweenFactorPose2(k-1, k , gtsam.Pose2(dodom(:,k)), odom_cov)); + end + + objpos = [obj.objects.pos]; + for k=1:length(obj.objects) + estimates.insert(obj.objects(k).id+1000, gtsam.Pose2([objpos(:,k);0])); + end + id1=obj.measurements.id1; + id2=obj.measurements.obj_id; + for k=1:length(id1) + factorgraph.add(gtsam.BetweenFactorPose2(... + id1(k), id2(k)+1000, gtsam.Pose2([obj.measurements.dpos(:,1);0]), ms_cov)); + end + + marginals = gtsam.Marginals(factorgraph,estimates); + Iodom=zeros(size(dodom,2),1); + for k=1:size(dodom,2) + Iodom(k) = log(det(marginals.marginalCovariance(k))); + end + Iobj=zeros(length(obj.objects),1); + for k=1:length(obj.objects) + covar = marginals.marginalCovariance(obj.objects(k).id+1000); + Iobj(k) = log(det(covar(1:2,1:2))); + end + end + + function [err_odom, err_obj]=computeError(obj, true_traj,true_obj) + est_traj = [obj.odoms.pos]; + Nodom = min([length(true_traj) length(obj.odoms.pos)]); + true_traj(3,:)=0; true_traj=true_traj(:,1:Nodom); + est_traj(3,:)=0; est_traj=est_traj(:,1:Nodom); + [T] = estimateRigidTransform(true_traj,est_traj); + est_traj(4,:)=1; est_traj = T*est_traj; + err_odom = sum( (true_traj(1:2,:)-est_traj(1:2,:)).^2,1); + + idx = obj.measurements.obj_id>0; + tobj = true_obj(:,obj.measurements.id2(idx)); + tobj(3,:)=0; + + objpos = [obj.objects.pos]; + for i=1:length(obj.objects) + id2idx(obj.objects(i).id)=i; + end + eobj = objpos(:,id2idx(obj.measurements.obj_id(idx))); + eobj(3,:)=0; eobj(4,:)=1; + eobj = T*eobj; + err_obj = sum( (tobj(1:2,:)-eobj(1:2,:)).^2,1); + end + +end +end + +function [ rectsLTWH ] = RectLTRB2LTWH( rectsLTRB ) +%rects (l, t, r, b) to (l, t, w, h) +rectsLTWH = [rectsLTRB(:, 1), rectsLTRB(:, 2), rectsLTRB(:, 3)-rectsLTRB(:,1)+1, rectsLTRB(:,4)-rectsLTRB(:,2)+1]; +end + +function rpy = rot2angle(R) +% QUAT2ROT +% R = QUAT2ROT(Q) converts a quaternion (4x1 or 1x4) into a 3x3 rotation mattrix +% +% reference: google! +% +% Babak Taati, 2003 +% (revised 2009) + +rpy(1) = atan2(R(3,2),R(3,3)); +rpy(2) = -asin(R(3,1)); +rpy(3) = atan2(R(2,1),R(1,1)); +end \ No newline at end of file diff --git a/data/my_simulation.mat b/data/my_simulation.mat new file mode 100644 index 0000000..6308951 Binary files /dev/null and b/data/my_simulation.mat differ diff --git a/data/my_simulation_2.mat b/data/my_simulation_2.mat new file mode 100644 index 0000000..2a71fc8 Binary files /dev/null and b/data/my_simulation_2.mat differ diff --git a/data/my_simulation_3.mat b/data/my_simulation_3.mat new file mode 100644 index 0000000..af5fe9a Binary files /dev/null and b/data/my_simulation_3.mat differ diff --git a/figures/fig_1.jpg b/figures/fig_1.jpg new file mode 100644 index 0000000..264d7b0 Binary files /dev/null and b/figures/fig_1.jpg differ diff --git a/figures/fig_2.jpg b/figures/fig_2.jpg new file mode 100644 index 0000000..ecb8070 Binary files /dev/null and b/figures/fig_2.jpg differ diff --git a/figures/fig_3.jpg b/figures/fig_3.jpg new file mode 100644 index 0000000..01041b1 Binary files /dev/null and b/figures/fig_3.jpg differ diff --git a/figures/fig_4.jpg b/figures/fig_4.jpg new file mode 100644 index 0000000..6796114 Binary files /dev/null and b/figures/fig_4.jpg differ diff --git a/figures/fig_5.jpg b/figures/fig_5.jpg new file mode 100644 index 0000000..af193c9 Binary files /dev/null and b/figures/fig_5.jpg differ diff --git a/figures/fig_6.jpg b/figures/fig_6.jpg new file mode 100644 index 0000000..89d8b7d Binary files /dev/null and b/figures/fig_6.jpg differ diff --git a/figures/fig_7.jpg b/figures/fig_7.jpg new file mode 100644 index 0000000..9ba5a6c Binary files /dev/null and b/figures/fig_7.jpg differ diff --git a/figures/loop_closure_plot.png b/figures/loop_closure_plot.png new file mode 100644 index 0000000..7ecbfd2 Binary files /dev/null and b/figures/loop_closure_plot.png differ diff --git a/figures/loop_closure_uncertain.png b/figures/loop_closure_uncertain.png new file mode 100644 index 0000000..c963df8 Binary files /dev/null and b/figures/loop_closure_uncertain.png differ diff --git a/figures/loop_closure_uncertain_true.png b/figures/loop_closure_uncertain_true.png new file mode 100644 index 0000000..8749e33 Binary files /dev/null and b/figures/loop_closure_uncertain_true.png differ diff --git a/generateSimData.m b/generateSimData.m index 1dd48c0..9e6c508 100644 --- a/generateSimData.m +++ b/generateSimData.m @@ -1,21 +1,23 @@ clear all; %% generate 2D objects truth_objects = rand(15,2)*10; -truth_objects(:,3)=mod(1:15,5); +truth_objects(:,3) = mod(1:15, 5); +%truth_objects(:,3)=[1:5,1:5,1:5]; +truth_objects(:,3) = 5*(truth_objects(:,3) == 0) + truth_objects(:,3); %% plot 2D objects figure; hold on; set(0,'DefaultLineMarkerSize',10) for i=1:length(truth_objects) switch truth_objects(i,3) - case 0 + case 1 plot(truth_objects(i,1),truth_objects(i,2),'bo','MarkerFaceColor','b'); - case 1 - plot(truth_objects(i,1),truth_objects(i,2),'rd','MarkerFaceColor','r'); case 2 - plot(truth_objects(i,1),truth_objects(i,2),'ms','MarkerFaceColor','m'); + plot(truth_objects(i,1),truth_objects(i,2),'rd','MarkerFaceColor','r'); case 3 - plot(truth_objects(i,1),truth_objects(i,2),'g^','MarkerFaceColor',[0.2 1 0.2]); + plot(truth_objects(i,1),truth_objects(i,2),'ms','MarkerFaceColor','m'); case 4 + plot(truth_objects(i,1),truth_objects(i,2),'g^','MarkerFaceColor',[0.2 1 0.2]); + case 5 plot(truth_objects(i,1),truth_objects(i,2),'yp','MarkerFaceColor',[1 1 0.3],... 'MarkerSize',15); end @@ -25,18 +27,35 @@ [x,y] = ginput(); %% generate odometry -truth_traj=[]; -for i=2:length(x) - l = norm([x(i)-x(i-1) y(i)-y(i-1)]); - t=0:0.1:l; - xq=interp1([0 l],[x(i-1) x(i)],t,'spline'); - yq=interp1([0 l],[y(i-1) y(i)],t,'spline'); - truth_traj = [truth_traj; [xq' yq']]; -end +%truth_traj=[]; + +% for i=2:length(x) +% l = norm([x(i)-x(i-1) y(i)-y(i-1)]); +% t=0:0.1:l; +% xq=interp1([0 l],[x(i-1) x(i)],t,'spline'); +% yq=interp1([0 l],[y(i-1) y(i)],t,'spline'); +% truth_traj = [truth_traj; [xq' yq']]; +% end + +t= 1:length(x); +tq = 1:0.1:length(x); +slope0 = 0; slopeF = 0; +xq = spline(t, [slope0; x; slopeF], tq); +yq = spline(t, [slope0; y; slopeF], tq); +%figure; plot(x, y, 'o', xq, yq, ':.'); +truth_traj = [xq' yq']; + for i=1:length(truth_traj)-1 odoms(i,3)=atan2(truth_traj(i+1,2)-truth_traj(i,2),truth_traj(i+1,1)-truth_traj(i,1)); end + +truth_traj = truth_traj(1:end-1,:); + odoms(end,3)=odoms(end-1,3); +odoms(:,1:2) = truth_traj(1:end,1:2); + +truth_traj(1:end,3) = odoms(:,3); +truth_traj(end, 3) = truth_traj(end,3); %% for i=2:length(odoms) @@ -52,14 +71,36 @@ lm_edge.dpos=[]; lm_edge.label=[]; for i=2:length(odoms) + % This block of code find the landmarks which are in the field of + % vision of the robot at a particular point + + % This is a list of the distances of all the landmarks from the current + % position of the robot. Length of list = Total landmarks + % dx, dy = vector towards landmark wrt the object + % dtheta = angle of the vector wrt the car's body dx=truth_objects(:,1)-odoms(i,1); dy=truth_objects(:,2)-odoms(i,2); dtheta = mod(atan2(dy,dx)-odoms(i,3)+pi,2*pi)-pi; R = [cos(odoms(i,3)),sin(odoms(i,3)); -sin(odoms(i,3)) cos(odoms(i,3))]; idx = find((dx.^2+dy.^2)0 label{end+1}=['class ' num2str(i)]; switch i @@ -37,6 +41,7 @@ end end axis equal; axis off; +title("True trajectory") legend(label); %% non-parametric diff --git a/sandbox/data/waypoints.mat b/sandbox/data/waypoints.mat new file mode 100644 index 0000000..e7113d4 Binary files /dev/null and b/sandbox/data/waypoints.mat differ diff --git a/sandbox/generateTraj.m b/sandbox/generateTraj.m new file mode 100644 index 0000000..37ebc41 --- /dev/null +++ b/sandbox/generateTraj.m @@ -0,0 +1,7 @@ +load('data/waypoints.mat'); +t= 1:length(x); +tq = 1:0.1:length(x); +slope0 = 0; slopeF = 0; +xq = spline(t, [slope0; x; slopeF], tq); +yq = spline(t, [slope0; y; slopeF], tq); +figure; plot(x, y, 'o', xq, yq, ':.'); \ No newline at end of file