cycling/interp_paths.m

   1 function new_paths = interp_paths(ps,p_param)
   2 %function adds new points along paths
   3 new_paths = ps;
   4
   5 pt_dist = 2000;
   6 %save easy named variables from struct
   7 pts = ps.grid_pts;
   8 pts(:,3) = ps.points(:,3);
   9 idx = ps.idx;
  10
  11 n = length(ps.paths);
  12 %number of new points on segment
  13 np = 2;
  14 new_points = [];
  15 news = [];
  16 new_idx = ps.idx;
  17 new_grid_pts = ps.grid_pts;
  18 new_points = ps.points;
  19 new_points2 = ps.points;
  20 new_grids = [];
  21 min_path_points = 2;
  22
  23 for i = 1:n
  24     p = ps.paths(i).p;
  25     new_p = p;
  26     pl = length(p);
  27     if pl >= min_path_points
  28         %fprintf('\n Making new points, path %d\n',i)
  29 %         if i == 280
  30 %             pause
  31 %         end
  32
  33         for j = 2:pl
  34
  35             %two points in space, p and q
  36             p_i = idx(p(j-1),1);%+rm*round(randn);
  37             p_j = idx(p(j-1),2);%+rm*round(randn);
  38             q_i = idx(p(j),1);%+rm*round(randn);
  39             q_j = idx(p(j),2);%+rm*round(randn);
  40             %distance between points in path
  41             dist = ps.raw_dist(p(j-1),p(j));
  42             %ts_4 --> dist/2000
  43             %ts_5 --> dist/1000
  44             np = round(dist/pt_dist)+1;
  45             %fprintf('Segment %d distance: %f new points: %d \n',j-1,dist,np);
  46             %linear interpolation of new points along the line
  47             new_lats = linspace(pts(p(j-1),1),pts(p(j),1),np)' ...
  48                 + 1/1000*randn(np,1);
  49             new_lons = linspace(pts(p(j-1),2),pts(p(j),2),np)' ...
  50                 + 1/1000*randn(np,1);
  51             new_time = linspace(pts(p(j-1),3),pts(p(j),3),np)';
  52             %average confidence of each endpoint on segment
  53             new_conf = mean(ps.points(j-1,4),ps.points(j,4))*ones(np,1);
  54             new_frps = linspace(ps.points(j-1,4),ps.points(j,4),np)';
  55             new_gran = ps.points(j-1,6)*ones(np,1);
  56             news = [news;[new_lats,new_lons,new_time]];
  57             %new_grids = [new_grids ;fixpoints2grid(ps.red,[new_lats,new_lons])];
  58             %figure(33),hold on,scatter3(new_points(:,3),new_points(:,4),new_time)
  59             %new_p =
  60             new_points=[new_points;[new_lats(2:end-1),new_lons(2:end-1), ...
  61                                     new_time(2:end-1),new_conf(2:end-1),new_frps(2:end-1),new_gran(2:end-1)]];
  62             %new_grid_pts = [new_grid_pts; [new_grids(2:end-1,3),new_grids(2:end-1,4)]];
  63             %new_idx = [new_idx; [new_grids(2:end-1,1),new_grids(2:end-1,2)]];
  64             %adding row position of new path points in case its needed
  65             %later
  66             idx_end = length(new_points);
  67             idx_start = idx_end-np+3;
  68             %new_p = [new_p(1:j-1) idx_start:idx_end new_p(j:end)];
  69             new_p = [new_p(1:find(new_p==p(j-1))) idx_start:idx_end new_p(find(new_p==p(j)):end)];
  70
  71         end
  72
  73     end
  74     %interpolate the whole path with spline
  75     new_paths.paths(i).p = new_p;
  76     if pl > 1
  77         pplat = csaps(ps.points(p,3),ps.points(p,1),p_param,new_points(new_p,3));
  78         pplon = csaps(ps.points(p,3),ps.points(p,2),p_param,new_points(new_p,3));
  79         new_points(new_p,1)=pplat;
  80         new_points(new_p,2)=pplon;
  81     end
  82 end % for i
  83 new_grids = fixpoints2grid(ps.red,[new_points(:,1),new_points(:,2)]);
  84 new_paths.grid_pts = [new_grids(:,3),new_grids(:,4)];
  85 new_paths.idx = [new_grids(:,1),new_grids(:,2)];
  86 new_paths.points = new_points;
  87 end % function