Difference in Cart2FRT and FRT2Cart in transformation.m

[imanask]

Hi Franz,

I would first like to thank you for building such a repo. I was trying out the transformation.m file. At the end of the code, we compare transform from Cartesian to Frenet and back to Cartesian. The results are not the same and we have some error in both the x and y positions. I thought this could be due to the ds=0.1. However, reducing ds by orders of magnitude did not help much in reducing the error. Is there any specific reason for this or is there a bug in the code that we need to look for?

Thanks,
Iman

[fjp]

Hi Iman,

Unfortunately I cannot replicate this issue as I don't have a running Matlab license at the moment. How large is the error between the results and does it stay similar when you try different distances from the reference path and curvatures?

Best regards
Franz

[imanask]

I have been using the same transformation.m code. The errors vary with the query position. The error is greater in the x direction. I have attached the results for two query objects. I first use Cart2FRT and then use its output as input to FRT2Cart and calculate the error between the chosen point and the and the result after transforming back and forth. Both these of the errors are for the case where ds = 0.001 (i.e. the length of faRefX and faRefY are 50933). For instance, the error for the second obstacle is more than 10 cm in x direction and 0.58 cm in y direction.

[fjp]

I ran a slightly modified version of transformation.m and can confirm that there is a rather large error in the x direction. I am quite certain that the error happens during the transformation from Frenet to Cartesian coordinates:

The calculated fSegmentX variable "reaches too far" (should be close to where the start of the arrow is), also i_faRefX(nPrevRefPoint) is too large. To me it seems that something isn't right with the running length of the reference path. Unfortunately I don't have time to look into this any further in the near future. I would suggest to look at the implementation of the reference path. Maybe it also makes sense to use a clothoid (continuous running length) instead of the sampled spline with its discrete running length.

Here is the code I used for transformation.m without the plotting (for the arrow I uncommented the quiver commands):

clear all
clc
set(0, 'DefaultLineLineWidth', 1);
disp('Optimal Frenet Path Planning')

% way points
WPx = [00.0, 20.0, 30.0, 50.0];
WPy = [00.0, 00.75, 02.15, 03.0];

WPx = [00.0, 20.0, 30.0, 50.0];
WPy = [00.0, 00.5, 04.5, 07.0];

%% Define obstacles in objects lists
% objx = objects(:,1);
% objy = objects(:,2);
faObjects = [10.0, -0.5;
    15.0, 2.0;
    20.0, 0.0;
    30.0, 3.0;
    45.0, 3.5];

%% Create a reference path
ds = 0.01;
GenerateTargetCourse = @(wx, wy) CalcSplineCourse(wx, wy, ds);
[faRefX, faRefY, faRefYaw, faRefCurvature, faRefRunLength, oReferencePath] = ...
    GenerateTargetCourse(WPx, WPy);


fObjX = faObjects(1,1)
fObjY = faObjects(1,2)

figure
plot(fObjX, fObjY, 'rx')
hold on
plot(faRefX, faRefY, 'x')


[fObjS, fObjD] = Cart2FRT(fObjX, fObjY, 0, faRefX, faRefY);
fObjS
fObjD
[fX, fY] = FRT2Cart(fObjS, fObjD, faRefRunLength, faRefX, faRefY)

[elwaaamr]

How Can I transformation from Frenet coordinates to Cartesian coordinates?
What are the equations that describe the transform from Frenet coordinates (longitudinal and Lateral) to Cartesian coordinates (X and Y)?
In another words, suppose I have (longitudinal displacement, lateral deviation, orientation and curvature) for the vehicle, then How can I get the XY position that represent those states?

[fjp]

The short answer is that you need to locate yourself on your reference frame with the s and d coordinates. Another important point is that you should have X and Y information given by your reference frame.

[elwaaamr]

@fjp, thanks for your replay.
Actually, I did some mathematics that based on taking the orientation of the reference path as input to the vehicle, and the output result is longitudinal displacement for the vehicle, lateral deviation for the vehicle curvature for the vehicle and the orientation of the vehicle. Now I need to plot the actual path of the vehicle regarding to the reference path. But I do not have any clue on how to use those state in order to plot the actual path of the vehicle in XY frame.

[imanask]

Hi all,
I corrected the error in the both the transformation functions, and they work fine now for me. Thanks for your code, it did help me get an understanding and made the implementation of the Frenet system more seamless. I have attached the codes both for FRT2Cart and Cart2FRT. Idon't remember what exactly I changed, but the error in the x-coordinate is no existent now @fjp you can take a look at them and if they look fine to you, you could merge them into the repo.
Corrected_Frenet_transforms.zip

[fjp]

@imanask thanks a lot for sharing your corrected solution. If I ever find time to work on this again I would be very interested to add some unit tests for FRT2Cart and Cart2FRT to see the actual difference. Using git diff at a glance I couldn't find an obvious issue yet.

@elwaaamr I am not sure I understand your issue, but if you have (an array of) the s (longitudinal) and d (lateral) coordinates, together with your reference path that is given the the x-y-frame, then it should be straight forward to use the Frenet2Cart function to calculate the actual path points of the vehicle. This function will output the vehicle locations in x-y frame which should be the result you are looking for.