Plot lines with curves from csv file

Hi,

I have a csv/txt file that contains x,y,z and also the rotation center x and rotation center y data. The data are basically the coordinates of a path that a robot follows. The robot can make his path in a straight line and also in curves. How can I use the rotation center x and y coordinates in my data to plot the curves.

My current plot has only straight lines. But as you can see in the image attached, the actual path inlcudes curves around corners based on the rotation center x and y data. The following code is obviously not enough.

figure(1)
plot(Data.PosX,Data.PosY)

[Konvictus177]

The path follows a circle (curve) around the center points in x and y.

The rotation centers x and y are the coordinates in x and y of the circle center relative to the current position (current position = the row before the row with entries in rotation center x and y coordinates).

So for example in the data the move from the 2nd to 3rd row would be a circular movement from x(2) to x(3) and from y(2) to y(3) but in a cirle with center point at x(2)+RotCenterX(3) and y(2)+RotCenterY(3).

[dpb]

You'll have to compute and draw line segments short enough between the two end points that they appear to be circular. One real weakness in MATLAB is and has always been there is only Rectangle that draws a circular object as one (certainly nonintuitive) option of its use.

To see the "how" to draw a circular arc see the FEX submission <circle>. It doesn't have the ability as written to do other than draw the whole circle with a given number of points, but you can modify it such that you set the rotation angle and start position to only draw the portion needed. That, of course is easy if you know the lines two points are connectin are at right angles, but may not be so simple for the little detours along the route. You might there have to figure that out as well by finding where the arc actually intersects the next point which will not necessarily be an exact match given you have to calculate in a discrete number of points, not continuously.

All in all, while it would be doable, it's going to take a fair amount of code to manage it.

I didn't do an exhaustive search on FEX to see if anybody has done something similar already, maybe. It does seem as though it might be a relatively common need in robotics mapping...

Answer 1

[Mathieu NOE]

maybe this ? I'm surprised to be lucky on monday morning

rounded corners appears now as red arcs of circle , but there's a bit of work if you don't want to see the original linear segments (at the corners)

Data = readtable('Test.csv');
indR = find((abs(Data.RotCenterX) + abs(Data.RotCenterY))>eps); % find occurences when we have to create a rounded corner

% create a full circle with 1 deg resolution
n = 360;
th = (0:n-1)/n*2*pi;
    
figure(1)
plot(Data.PosX,Data.PosY,'-*')
hold on

for k = 1: numel(indR)
    indRyk = indR(k);
    xc = Data.PosX(indRyk-1) + Data.RotCenterX(indRyk);
    yc = Data.PosY(indRyk-1) + Data.RotCenterY(indRyk);
    R = sqrt(Data.RotCenterX(indRyk)^2 + Data.RotCenterY(indRyk)^2);

    % create circle
    xCir = R*cos(th) + xc;
    yCir = R*sin(th) + yc;
    % limit circle to valid segment (within range of X,Y from start to end point)
    indX = (xCir>=min(Data.PosX(indRyk-1),Data.PosX(indRyk))) & (xCir<=max(Data.PosX(indRyk-1),Data.PosX(indRyk)));
    indY = (yCir>=min(Data.PosY(indRyk-1),Data.PosY(indRyk))) & (yCir<=max(Data.PosY(indRyk-1),Data.PosY(indRyk)));
    indXY = indX & indY;
    xCir = xCir(indXY);
    yCir = yCir(indXY);
    plot(xCir,yCir,'r.');
end

hold off

[Mathieu NOE]

An improved code , so in the turns you don't see anymore the original linear segments

Data = readtable('Test.csv');
indR = find((abs(Data.RotCenterX) + abs(Data.RotCenterY))>eps); % find occurences when we have to create a rounded corner
indN = 1:size(Data,1);
% create a full circle with 1 deg resolution
n = 360;
th = (0:n-1)/n*2*pi;
    
figure(1)
hold on

% init
indRyk_old = 0;

for k = 1: numel(indR)
    
    indRk = indR(k);% current index
    
    % plot the linear segment first
    ind = find(indN<indRk & indN>=indRyk_old);
    plot(Data.PosX(ind),Data.PosY(ind),'b-','linewidth',1.5)
    
    % then plot the turn (arc of circle)
    % circle center (xc,yc) and radius R
    xc = Data.PosX(indRk-1) + Data.RotCenterX(indRk);
    yc = Data.PosY(indRk-1) + Data.RotCenterY(indRk);
    R = sqrt(Data.RotCenterX(indRk)^2 + Data.RotCenterY(indRk)^2);

    % create circle
    xCir = R*cos(th) + xc;
    yCir = R*sin(th) + yc;
    % limit circle to valid segment (within range of X,Y from start to end point)
    indX = (xCir>=min(Data.PosX(indRk-1),Data.PosX(indRk))) & (xCir<=max(Data.PosX(indRk-1),Data.PosX(indRk)));
    indY = (yCir>=min(Data.PosY(indRk-1),Data.PosY(indRk))) & (yCir<=max(Data.PosY(indRk-1),Data.PosY(indRk)));
    indXY = indX & indY;
    xCir = xCir(indXY);
    yCir = yCir(indXY);
    plot(xCir,yCir,'.r');
    
    % update indRyk_old
    indRyk_old = indRk;
end

hold off

[Mathieu NOE]

yet another improvement - up to now I was using dots to plot the red arcs of circle, as I faced with the fact that I could have a jump if I would use the line plot , like this :

a simple fix to that is to generate the base circle with angular position shifted by half the angular step :

% th = (0:n-1)/n*2*pi; % this is fine but can generate zig zags in the red cornes (because last point is in fact the first)
th = (0.5+(0:n-1))/n*2*pi; % a simple trick to avoid such situation

full code :

Data = readtable('Test.csv');
indR = find((abs(Data.RotCenterX) + abs(Data.RotCenterY))>eps); % find occurences when we have to create a rounded corner
indN = 1:size(Data,1);
% create a full circle with 1 deg resolution
n = 360;
% th = (0:n-1)/n*2*pi; % this is fine but can generate zig zags in the red cornes (because last point is in fact the first)
th = (0.5+(0:n-1))/n*2*pi; % a simple trick to avoid such situation 
    
figure(1)
hold on

% init
indRyk_old = 0;

for k = 1:numel(indR)
    
    indRk = indR(k);% current index
    
    % plot the linear segment first
    ind = find(indN<indRk & indN>=indRyk_old);
    plot(Data.PosX(ind),Data.PosY(ind),'b-','linewidth',1.5)
    
    % then plot the turn (arc of circle)
    % circle center (xc,yc) and radius R
    xc = Data.PosX(indRk-1) + Data.RotCenterX(indRk);
    yc = Data.PosY(indRk-1) + Data.RotCenterY(indRk);
    R = sqrt(Data.RotCenterX(indRk)^2 + Data.RotCenterY(indRk)^2);

    % create circle
    xCir = R*cos(th) + xc;
    yCir = R*sin(th) + yc;
    % limit circle to valid segment (within range of X,Y from start to end point)
    indX = (xCir>=min(Data.PosX(indRk-1),Data.PosX(indRk))) & (xCir<=max(Data.PosX(indRk-1),Data.PosX(indRk)));
    indY = (yCir>=min(Data.PosY(indRk-1),Data.PosY(indRk))) & (yCir<=max(Data.PosY(indRk-1),Data.PosY(indRk)));
    indXY = (indX & indY);
    xCir = xCir(indXY);
    yCir = yCir(indXY);
    plot(xCir,yCir,'r','linewidth',1.5);
    
    % update indRyk_old
    indRyk_old = indRk;
end

hold off

[Mathieu NOE]

well, seems today is a good day , so let's move on.

last (but not least ?) suggestion. ... I was wondering if the best option is not simply to re-create the x,y data and then plot them (after the for loop) , instead of doing the plots one after the other in the for loop.

so , probably now the best code I can provide today . Notice that the angular resolution of the circle and if we use or not the half sample angular shift has no more any importance .

here I opted for a 10 deg resolution, maybe that's enough for a good visual rendering but you can of course change that for whatever prefered value

enjoy !!

%% load data
Data = readtable('Test.csv');
indR = find((abs(Data.RotCenterX) + abs(Data.RotCenterY))>eps); % find occurences when we have to create a rounded corner
indN = 1:size(Data,1);

%% init
indRyk_old = 0;
x = [];
y = [];
% create a full circle with 10 deg resolution
n = 36;
th = (0:n-1)/n*2*pi; % option #1 
% th = (0.5+(0:n-1))/n*2*pi; % option #2  whatever your prefer

%% main loop
for k = 1:numel(indR)
    
    indRk = indR(k);% current index
    
    % first extract x,y data of the linear segment 
    ind = find(indN<indRk & indN>=indRyk_old);
    x = [x; Data.PosX(ind)];
    y = [y; Data.PosY(ind)];
    
    % then create the turn (arc of circle)
    % circle center (xc,yc) and radius R
    xc = Data.PosX(indRk-1) + Data.RotCenterX(indRk);
    yc = Data.PosY(indRk-1) + Data.RotCenterY(indRk);
    R = sqrt(Data.RotCenterX(indRk)^2 + Data.RotCenterY(indRk)^2);

    % create circle
    xCir = R*cos(th) + xc;
    yCir = R*sin(th) + yc;
    
    % limit circle to valid segment (within range of X,Y from start to end point)
    indX = (xCir>=min(Data.PosX(indRk-1),Data.PosX(indRk))) & (xCir<=max(Data.PosX(indRk-1),Data.PosX(indRk)));
    indY = (yCir>=min(Data.PosY(indRk-1),Data.PosY(indRk))) & (yCir<=max(Data.PosY(indRk-1),Data.PosY(indRk)));
    indXY = (indX & indY);
    xCir = xCir(indXY);
    yCir = yCir(indXY);
    
    % add the circle points but pay attention to take them in the right order : 
    % from lowest to highest distance to end point of the linear segment (before the arc circle)
    d = (xCir-x(end)).^2+(yCir-y(end)).^2;
    [val,ind] = sort(d,'ascend');
    x = [x; xCir(ind)'];
    y = [y; yCir(ind)'];
    
    % update indRyk_old
    indRyk_old = indRk;
end

% plot data
plot(x,y,'b-','linewidth',1.5)
axis equal

[Konvictus177]

Wow, this looks amazing!! I appreciate the effort you put into this!!

I wasn't able to go through your code yet however, based on the image you shared the last line is missing in the plot. I will try to find the root cause when going through your code.

My "old" code with sharp corners and the missing portion below:

[Mathieu NOE]

hello again

yes you're correct , after changing the code structure, the last straigth line was lost

this is now corrected :

%% load data
Data = readtable('Test.csv');
indR = find((abs(Data.RotCenterX) + abs(Data.RotCenterY))>eps); % find occurences when we have to create a rounded corner
indN = 1:size(Data,1);

%% init
indRk_old = 0;
x = [];
y = [];
% create a full circle with 10 deg resolution
n = 36;
th = (0:n-1)/n*2*pi; % option #1 
% th = (0.5+(0:n-1))/n*2*pi; % option #2  whatever your prefer

%% main loop
for k = 1:numel(indR)
    
    indRk = indR(k);% current index
    
    % first extract x,y data of the linear segment 
    ind = find(indN<indRk & indN>=indRk_old);
    x = [x; Data.PosX(ind)];
    y = [y; Data.PosY(ind)];
    
    % then create the turn (arc of circle)
    % circle center (xc,yc) and radius R
    xc = Data.PosX(indRk-1) + Data.RotCenterX(indRk);
    yc = Data.PosY(indRk-1) + Data.RotCenterY(indRk);
    R = sqrt(Data.RotCenterX(indRk)^2 + Data.RotCenterY(indRk)^2);

    % create circle
    xCir = R*cos(th) + xc;
    yCir = R*sin(th) + yc;
    
    % limit circle to valid segment (within range of X,Y from start to end point)
    indX = (xCir>=min(Data.PosX(indRk-1),Data.PosX(indRk))) & (xCir<=max(Data.PosX(indRk-1),Data.PosX(indRk)));
    indY = (yCir>=min(Data.PosY(indRk-1),Data.PosY(indRk))) & (yCir<=max(Data.PosY(indRk-1),Data.PosY(indRk)));
    indXY = (indX & indY);
    xCir = xCir(indXY);
    yCir = yCir(indXY);
    
    % add the circle points but pay attention to take them in the right order : 
    % from lowest to highest distance to end point of the linear segment (before the arc circle)
    d = (xCir-x(end)).^2+(yCir-y(end)).^2;
    [val,ind] = sort(d,'ascend');
    x = [x; xCir(ind)'];
    y = [y; yCir(ind)'];
    
    % update indRyk_old
    indRk_old = indRk;
end

% there is maybe a last straight portion, so don't forget it
if indN(end)>indRk
    x = [x; Data.PosX(end)];
    y = [y; Data.PosY(end)];
end

% plot data
plot(x,y,'b-','linewidth',1.5)
axis equal

●

[Konvictus177]

Ok found the issue. The last line segments after the last occurence of a found rounded corner had to be added do x and y. However, I think there might still be something not fully correct. I will give an update with another "TestFile" later.

[Konvictus177]

So for this Test2.csv file for example I sometimes do not have rounded cornes as you can see in the picture. The picture below shows an example where I am supposed to have an almost semi circle but the red dotted line ends abruptly. This happens more than once. I will try to go thorugh your code to find the root cause for this. I think it has something to do with the "limit circle to valid segments".

[Konvictus177]

Ok, so I think I am right. It has something to do with the limit for the circle segments. When plotting the full circles the missing segment is there.

This is the section from the example above where the red dottes line ends abruptly:

[Mathieu NOE]

yep

I am working on it

have to change the logic how to get the right portion of the circle

[Mathieu NOE]

believed I have found the most straigthforward way to change the sharp corners into arc circles without complicated tests on which portion of the circle must be kept ...

but I still think there is a bit of work to do :

here a code with overlaid "raw" data with sharp corners in blue and the "new" data in red dots

with the first data file it works fine

with the second data it gives just one strange result in the beginning , because of whatever sharp zig zag

%% load data
Data = readtable('Test.csv');
indR = find((abs(Data.RotCenterX) + abs(Data.RotCenterY))>eps); % find occurences when we have to create a rounded corner
indN = 1:size(Data,1);

figure(1)
plot(Data.PosX,Data.PosY,'b-')
hold on

%% init
Np = 25; % number of points for each arc segment 
indRk_old = 0;
x = [];
y = [];

%% main loop
for k = 1:numel(indR)
    
    indRk = indR(k);% current index
    
    % first extract x,y data of the linear segment 
    ind = find(indN<indRk & indN>=indRk_old);
    x = [x; Data.PosX(ind)];
    y = [y; Data.PosY(ind)];
    
    dx = x(end) - x(end-1);
    dy = y(end) - y(end-1);
    
    % then create the turn (arc of circle)
    % circle center (xc,yc) and radius R
    xc = Data.PosX(indRk-1) + Data.RotCenterX(indRk);
    yc = Data.PosY(indRk-1) + Data.RotCenterY(indRk);
    R = sqrt(Data.RotCenterX(indRk)^2 + Data.RotCenterY(indRk)^2);

    % get the polar coordinates of starting and ending point of the arc
    ie = ind(end);
    [th1,r1] = cart2pol(Data.PosX(ie)-xc,Data.PosY(ie)-yc); % starting point
    [th2,r2] = cart2pol(Data.PosX(ie+1)-xc,Data.PosY(ie+1)-yc); % ending point
    % NB : R = r1 = r2 
  
   % create first a straigth line  (x,y array) between starting and ending point of the arc
   xx = linspace(Data.PosX(ie),Data.PosX(ie+1),Np);
   yy = linspace(Data.PosY(ie),Data.PosY(ie+1),Np);
   % now convert this line into an arc of circle
   [th,r] = cart2pol(xx-xc,yy-yc); % 
   [xCir,yCir] = pol2cart(th,r2); % convert back with correct radius = r2 (or r1 or R if you prefer)
   xCir = xCir+xc;
   yCir = yCir+yc;
  
    x = [x; xCir(:)];
    y = [y; yCir(:)];
    
    % update indRyk_old
    indRk_old = indRk;
end

% there is maybe a last straight portion, so don't forget it
if indN(end)>indRk
    x = [x; Data.PosX(end)];
    y = [y; Data.PosY(end)];
end

% plot new data with round corners
% plot(x,y,'b-','linewidth',1.5)
plot(x,y,'r.','linewidth',1.5)
axis equal

[Mathieu NOE]

seems the code has a problem with the first 10 samples of the second file

if I remove them I have no more problems

%% load data
Data = readtable('Test2c.csv');
indR = find((abs(Data.RotCenterX) + abs(Data.RotCenterY))>eps); % find occurences when we have to create a rounded corner
indN = 1:size(Data,1);

figure(1)
plot(Data.PosX,Data.PosY,'b-')
hold on

%% init
Np = 25; % number of points for each arc segment 
indRk_old = 0;
x = [];
y = [];

%% main loop
for k = 1:numel(indR)
    
    indRk = indR(k);% current index
    
    % first extract x,y data of the linear segment 
    ind = find(indN<indRk & indN>=indRk_old);
    x = [x; Data.PosX(ind)];
    y = [y; Data.PosY(ind)];
    
    
    % then create the turn (arc of circle)
    % circle center (xc,yc) and radius R
    xc = Data.PosX(indRk-1) + Data.RotCenterX(indRk);
    yc = Data.PosY(indRk-1) + Data.RotCenterY(indRk);
    R = sqrt(Data.RotCenterX(indRk)^2 + Data.RotCenterY(indRk)^2);

    % get the polar coordinates of starting and ending point of the arc
    [th1,r1] = cart2pol(Data.PosX(indRk-1)-xc,Data.PosY(indRk-1)-yc); % starting point
    [th2,r2] = cart2pol(Data.PosX(indRk)-xc,Data.PosY(indRk)-yc); % ending point
    % NB : R = r1 = r2 
  
   % create first a straigth line  (x,y array) between starting and ending point of the arc
    xx = linspace(Data.PosX(indRk-1),Data.PosX(indRk),Np);
   yy = linspace(Data.PosY(indRk-1),Data.PosY(indRk),Np);
   % now convert this line into an arc of circle
   [th,r] = cart2pol(xx-xc,yy-yc); % 
   [xCir,yCir] = pol2cart(th,r2); % convert back with correct radius = r2 (or r1 or R if you prefer)
   xCir = xCir+xc;
   yCir = yCir+yc;
  
    x = [x; xCir(:)];
    y = [y; yCir(:)];
    
    % update indRyk_old
    indRk_old = indRk;
end

% there is maybe a last straight portion, so don't forget it
if indN(end)>indRk
    x = [x; Data.PosX(end)];
    y = [y; Data.PosY(end)];
end

% plot new data with round corners
% plot(x,y,'b-','linewidth',1.5)
plot(x,y,'r.','linewidth',1.5)

●

% axis equal

[Konvictus177]

This is absolutely amazing!! The Test2.csv file had a wrong x,y position in one of the rows at the top. This was my mistake. Attached you can find the corrected .csv file Test3.csv.

I added arrows in the final plot with quiver to see the path direction and I made a minor change regarding the last missing straight portion. I think its supposed to be:

% there is maybe a last straight portion, so don't forget it
if indN(end)>indRk
    x = [x; Data.PosX(indRk+1:end)];
    y = [y; Data.PosY(indRk+1:end)];
end

Here the final code I used:

%% load data
Data = readtable('Test3.csv');

indR = find((abs(Data.RotCenterX) + abs(Data.RotCenterY))>eps); % find occurences when we have to create a rounded corner
indN = 1:size(Data,1);
figure(1)
plot(Data.PosX,Data.PosY,'b-')
axis equal
%% init
%% init
Np = 25; % number of points for each arc segment 
indRk_old = 0;
x = [];
y = [];
%% main loop
for k = 1:numel(indR)
    
    indRk = indR(k);% current index
    
    % first extract x,y data of the linear segment 
    ind = find(indN<indRk & indN>=indRk_old);
    x = [x; Data.PosX(ind)];
    y = [y; Data.PosY(ind)];
    
    dx = x(end) - x(end-1);
    dy = y(end) - y(end-1);
    
    % then create the turn (arc of circle)
    % circle center (xc,yc) and radius R
    xc = Data.PosX(indRk-1) + Data.RotCenterX(indRk);
    yc = Data.PosY(indRk-1) + Data.RotCenterY(indRk);
    R = sqrt(Data.RotCenterX(indRk)^2 + Data.RotCenterY(indRk)^2);
    % get the polar coordinates of starting and ending point of the arc
    ie = ind(end);
    [th1,r1] = cart2pol(Data.PosX(ie)-xc,Data.PosY(ie)-yc); % starting point
    [th2,r2] = cart2pol(Data.PosX(ie+1)-xc,Data.PosY(ie+1)-yc); % ending point
    % NB : R = r1 = r2 
  
   % create first a straigth line  (x,y array) between starting and ending point of the arc
   xx = linspace(Data.PosX(ie),Data.PosX(ie+1),Np);
   yy = linspace(Data.PosY(ie),Data.PosY(ie+1),Np);
   % now convert this line into an arc of circle
   [th,r] = cart2pol(xx-xc,yy-yc); % 
   [xCir,yCir] = pol2cart(th,r2); % convert back with correct radius = r2 (or r1 or R if you prefer)
   xCir = xCir+xc;
   yCir = yCir+yc;
  
    x = [x; xCir(:)];
    y = [y; yCir(:)];
    
    % update indRyk_old
    indRk_old = indRk;
end
% there is maybe a last straight portion, so don't forget it
if indN(end)>indRk
    x = [x; Data.PosX(indRk+1:end)];
    y = [y; Data.PosY(indRk+1:end)];
end
% plot new data with round corners
% figure(1)
% plot(x,y,'b-','linewidth',1.5)
% axis equal

% plot new data with round corners
figure(2)
% quiver(x(1:end-1),y(1:end-1), diff(x), diff(y), 0)
q=quiver(x(1:end-1),y(1:end-1), diff(x), diff(y), 0, 'r');
hold on
plot(x,y,'b-','linewidth',1.5)
q.MaxHeadSize=0.1;
axis equal

[Mathieu NOE]

yeap I think there is a problem at line 10 in your data file Test2.csv

the is a big jump in the Y position from -71.35 to -26.35 , but we are supposed to do only a turn of radius = 2 so this is not possible

I believe the Y value -71.35 is wrong . And if you look carefully at the first 10 samples plot it shows a very strange zig zag created by this "wrong" point.

all the other points where there is a rotation in place, you see that the x and y linear coordinates change is compatible with a r = 2 turn. But not in lines 10 and 11

[Konvictus177]

Yes, thats correct. After generating the .csv file I had to edit it manually and I made a mistake there. The Test3.csv file should be correct. Thanks again!

[Mathieu NOE]

as always, my pleasure !

glad we found the bug ! (and it's not the code :))

and the quiver plot is a good idea too

fyi I made a few clean up ops in the code , so there are some stuff I removed / changed a bit

%% load data
Data = readtable('Test3.csv');
indR = find((abs(Data.RotCenterX) + abs(Data.RotCenterY))>eps); % find occurences when we have to create a rounded corner
indN = 1:size(Data,1);

%% init
Np = 100; % number of points for each arc segment 
indRk_old = 0;
x = [];
y = [];

%% main loop
for k = 1:numel(indR)
    
    indRk = indR(k);% current index
    
    % first extract x,y data of the linear segment 
    ind = find(indN<indRk & indN>=indRk_old);
    x = [x; Data.PosX(ind)];
    y = [y; Data.PosY(ind)];
  
    % then create the turn (arc of circle)
    % circle center (xc,yc) and radius R
    xc = Data.PosX(indRk-1) + Data.RotCenterX(indRk);
    yc = Data.PosY(indRk-1) + Data.RotCenterY(indRk);
    R = sqrt(Data.RotCenterX(indRk)^2 + Data.RotCenterY(indRk)^2);
    % get the polar coordinates of starting and ending point of the arc
    [th1,r1] = cart2pol(Data.PosX(indRk-1)-xc,Data.PosY(indRk-1)-yc); % starting point
    [th2,r2] = cart2pol(Data.PosX(indRk)-xc,Data.PosY(indRk)-yc); % ending point
    % NB : R = r1 = r2 
  
   % create first a straigth line  (x,y array) between starting and ending point of the arc
   xx = linspace(Data.PosX(indRk-1),Data.PosX(indRk),Np);
   yy = linspace(Data.PosY(indRk-1),Data.PosY(indRk),Np);
   % now convert this line into an arc of circle
   [th,r] = cart2pol(xx-xc,yy-yc); % 
   [xCir,yCir] = pol2cart(th,r2); % convert back with correct radius = r2 (or r1 or R if you prefer)
   xCir = xCir+xc;
   yCir = yCir+yc;
  
    x = [x; xCir(:)];
    y = [y; yCir(:)];
    
    % update indRyk_old
    indRk_old = indRk;
end

% there is maybe a last straight portion, so don't forget it
if indN(end)>indRk
    x = [x; Data.PosX(indRk+1:end)];
    y = [y; Data.PosY(indRk+1:end)];
end

% plot new data with round corners
figure(2)
q=quiver(x(1:end-1),y(1:end-1), diff(x), diff(y), 0, 'r');
hold on
plot(x,y,'b-','linewidth',1.5)
q.MaxHeadSize=0.1;
axis equal