g1_fitting.cpp File Reference

#include <math.h>
#include <algorithm>
#include <cmath>
#include <sstream>
#include <stdexcept>
#include <vector>

Include dependency graph for g1_fitting.cpp:

Macros
#define	CLOTHOID_ASSERT(COND, MSG)

Functions
void	g1_fit::calc_std_fresnel_integral (double y, double &int_c, double &int_s)
void	g1_fit::calc_std_fresnel_integral_moments (int n_k, double t, double c_k[], double s_k[])
double	g1_fit::calc_lommel_term (double mu, double nu, double b)
void	g1_fit::calc_gen_fresnel_integral_moments (int n_k, double a, double b, double c, double x_k[], double y_k[])
double	g1_fit::normalize_abs_pi (double x)
double	g1_fit::calc_initial_guess (double phi0, double phi1)
double	g1_fit::find_root (double a_guess, double delta, double phi0, int niter_max, double tol)
double	g1_fit::calc_clothoid_length (double a, double delta, double phi0, double r)
void	g1_fit::calc_clothoid (double x0, double y0, double theta0, double x1, double y1, double theta1, double &k, double &dk, double &l)

Detailed Description

See also

g1_fitting.hpp

Macro Definition Documentation

CLOTHOID_ASSERT

#define CLOTHOID_ASSERT	(		COND,
			MSG
	)

Value:

  if (!(COND)) {                                                                    \
    std::ostringstream ost;                                                         \
    ost << "On line: " << __LINE__ << " file: " << __FILE__ << '\n' << MSG << '\n'; \
    throw std::runtime_error(ost.str());                                            \
  }

Function Documentation

calc_clothoid()

void g1_fit::calc_clothoid	(	double	x0,
		double	y0,
		double	theta0,
		double	x1,
		double	y1,
		double	theta1,
		double &	k,
		double &	dk,
		double &	L
	)

Compute clothoid parameters from a start and an endpoint. The clothoid curve is defined as x(s) = x0 + \int_0^s cos( 0.5*dk*t^2 + k*t + theta0 ) dt y(s) = y0 + \int_0^s sin( 0.5*dk*t^2 + k*t + theta0 ) dt

Refer to Eq. 1 in https://arxiv.org/pdf/1209.0910.pdf.

Parameters

x0	x(0) in equation above [m].
y0	y(0) in equation above [m].
theta0	Curve angle at s=0 [rad], theta(s)= 0.5*dk*t^2 + k*t + theta0.
x1	x(L) in equation above [m].
y1	y(L) in equation above [m].
theta1	Curve angle at s=L [rad].
k	Curvature at s=0 [1/m], see equation above.
dk	Curvature change [1/m^2], see equation above.
L	Clothoid length [m] from (x0, y0) to (x1, y1).

Here is the call graph for this function:

Here is the caller graph for this function:

calc_clothoid_length()

double g1_fit::calc_clothoid_length	(	double	a,
		double	delta,
		double	phi0,
		double	r
	)

Calculate clothoid length for a given root.

Parameters

a	Root of g(A) = \int_0^1 \sin( A*t^2+(delta-A)*t+phi0 ) dt.
delta	Angle used in the clothoid fitting problem.
phi0	Angle used in the clothoid fitting problem.
r	Distance between clothoid start and end point.

Here is the call graph for this function:

Here is the caller graph for this function:

calc_gen_fresnel_integral_moments()

void g1_fit::calc_gen_fresnel_integral_moments	(	int	n_k,
		double	a,
		double	b,
		double	c,
		double	x_k[],
		double	y_k[]
	)

Compute moments of Fresnel integrals: x_k(a,b,c) = \int_0^1 t^k * cos( (a/2)*t^2 + b*t + c ) dt y_k(a,b,c) = \int_0^1 t^k * sin( (a/2)*t^2 + b*t + c ) dt

Refer to Eq. 17 in https://arxiv.org/pdf/1209.0910.pdf.

Parameters

n_k	Number of moments to be computed.
a	See equation above.
b	See equation above.
c	See equation above.
x_k	Array of Fresnel cosine moments [x_0,x_1,...,x_{n_k-1}]
y_k	Array of Fresnel sine moments [y_0,y_1,...,y_{n_k-1}]

Here is the call graph for this function:

Here is the caller graph for this function:

calc_initial_guess()

double g1_fit::calc_initial_guess	(	double	phi0,
		double	phi1
	)

Find guess for roots of function g(A).

Inputs: Normalized angle used in the clothoid fitting problem.

Parameters

phi0	At clothoid start.
phi1	At clothoid end.

Here is the call graph for this function:

Here is the caller graph for this function:

calc_lommel_term()

double g1_fit::calc_lommel_term	(	double	mu,
		double	nu,
		double	b
	)

Compute Lommel function expansion: w(mu,nu,b) = \sum_0^\inf (-b^2)^n / alpha(n+1,mu,nu), where alpha(n,mu,nu) = \prod_1^n (mu + 2*m - 1)^2 - nu^2

Refer to Eq. 27 ff in https://arxiv.org/pdf/1209.0910.pdf.

Parameters

mu	See equation above.
nu	See equation above.
b	See equation above.

Here is the call graph for this function:

Here is the caller graph for this function:

calc_std_fresnel_integral()

void g1_fit::calc_std_fresnel_integral	(	double	y,
		double &	int_c,
		double &	int_s
	)

Compute standard Fresnel integrals: c(y) = \int_0^y cos( (pi/2)*x^2 ) dx s(y) = \int_0^y sin( (pi/2)*x^2 ) dx

Refer to Eq. 14 in https://arxiv.org/pdf/1209.0910.pdf.

The present algorithm is described in Thompson, W. J., 1997. Atlas for Computing Mathematical Functions: An Illustrated Guide for Practitioners with Programs in C and Mathematica with CDrom, 1st Edition. John Wiley & Sons, Inc., New York, NY, USA,

using the modifications proposed by Venkata Sivakanth Telasula (2023). Fresnel Cosine and Sine Integral Function (https://www.mathworks.com/matlabcentral/fileexchange/9017-fresnel-cosine-and-sine-integral-function), MATLAB Central File Exchange.

Parameters

y	See equation above.
int_c	Fresnel cosine integral.
int_s	Fresnel sine integral.

Here is the call graph for this function:

Here is the caller graph for this function:

calc_std_fresnel_integral_moments()

void g1_fit::calc_std_fresnel_integral_moments	(	int	n_k,
		double	t,
		double	c_k[],
		double	s_k[]
	)

Compute moments of Fresnel integrals: c_k(t) = \int_0^t s^k * cos( (pi/2)*s^2 ) ds s_k(t) = \int_0^t s^k * sin( (pi/2)*s^2 ) ds

Refer to Eq. 15 in https://arxiv.org/pdf/1209.0910.pdf.

Parameters

n_k	Number of moments to be computed.
t	See equation above.
c_k	Array of Fresnel cosine moments [c_0,c_1,...,c_{n_k-1}]
s_k	Array of Fresnel sine moments [s_0,s_1,...,s_{n_k-1}]

Here is the call graph for this function:

Here is the caller graph for this function:

find_root()

double g1_fit::find_root	(	double	a_guess,
		double	delta,
		double	phi0,
		int	niter_max,
		double	tol
	)

Find root of function g(A) defined as g(A) = \int_0^1 \sin( A*t^2+(delta-A)*t+phi0 ) dt

Parameters

a_guess	Initial guess.
delta	Angle used in the clothoid fitting problem.
phi0	Angle used in the clothoid fitting problem.
niter_max	Allowed number of Newton iterations.
tol	Max. allowed residual.

Here is the call graph for this function:

Here is the caller graph for this function:

normalize_abs_pi()

double g1_fit::normalize_abs_pi ( double  x )

inline

Normalize angle to range [-M_PI, M_PI].

Parameters

x	Arbitrary angle [rad].

Here is the call graph for this function:

Here is the caller graph for this function: