src/map/map.cpp
/**
    transform position in frenet frame to world frame using spline smoother.

    @param s frenet s.
    @param d frenet d.
    @return position in world frame.
*/
transform position in frenet frame to world frame using spline smoother.
transform position in frenet frame to world frame using spline smoother .
std::vector<double> Map::get_world_frame_position_smoothed(double s, double d)
{
    s = fmod(s, ROUND_TRIP_S);
    double dx = center_line_dx_smoothed(s);
    double dy = center_line_dy_smoothed(s);
    double norm_d = sqrt(dx*dx + dy*dy);
    dx /= norm_d;
    dy /= norm_d;
    double world_x = center_line_x_smoothed(s);
    double world_y = center_line_y_smoothed(s);
    world_x += d * dx;
    world_y += d * dy;
	return {world_x, world_y};
}
std :: vector < double > Map :: get_world_frame_position_smoothed ( double s , double d ) { s = fmod ( s , ROUND_TRIP_S ) ; double dx = center_line_dx_smoothed ( s ) ; double dy = center_line_dy_smoothed ( s ) ; double norm_d = sqrt ( dx * dx + dy * dy ) ; dx /= norm_d ; dy /= norm_d ; double world_x = center_line_x_smoothed ( s ) ; double world_y = center_line_y_smoothed ( s ) ; world_x += d * dx ; world_y += d * dy ; return { world_x , world_y } ; }
transform position in frenet frame to world frame using spline smoother.
transform position in frenet frame to world frame using spline smoother .
// rectify s: // estimate normal vector: // unit normal vector: // center line: // add normal shift: