get road id and s/t coordinates given xyz point

[saoraozhe3hao]

Is it possible that provide a function named get_roadid_st_list_by_xy in C++

[pageldev]

Please describe in more detail what function you need

[saoraozhe3hao]

I am sorry. I am not good at English. I am trying to describe clearly.
There is a function named get_xyz in Road.h. It return world xyz with inputting s and t.
We need the reverse function which return roadid,s,t with inputting xyz or return roadid,s,t list with inputting xy

[pageldev]

Ok. I understand. I will be adding that query feature.

[lrstttl]

Hello @grepthat, do you already have an update here?
I am also very interested in that feature.

[Jmingyu0822]

I am sorry. I am not good at English. I am trying to describe clearly. There is a function named get_xyz in Road.h. It return world xyz with inputting s and t. We need the reverse function which return roadid,s,t with inputting xyz or return roadid,s,t list with inputting xy

@saoraozhe3hao
Hi, you said get_xyz() function return world xyz , but in my case there is something wrong.. Coordinate is different from world coordinate. " tmp_xyz = odr_road.get_xyz(1.0, 0.0, 0.0); " I use this line to get world coordinate
What is the problem. plz let me know

[pageldev]

When loading the map via odr::OpenDriveMap odr_map("data.xodr"); the default option is to center the map. So you would probably need to add OpenDriveMap::x_offs and y_offs to your point to get the correct world coordinate.

[wangwei-anye]

Ok. I understand. I will be adding that query feature.

is here has the feature？

[JuddiW]

I‘ve implemented the query feature, but i don't konw how to sync it to github.

[lrstttl]

@JuddiW looks good, I would like to try if it works.
What do you mean by syncing? You can have a look at this guide: https://gist.github.com/Chaser324/ce0505fbed06b947d962

[JuddiW]

@lrstttl > @JuddiW looks good, I would like to try if it works. What do you mean by syncing? You can have a look at this guide: https://gist.github.com/Chaser324/ce0505fbed06b947d962

Thank you for your help, I will try it.

[rpf1019]

@lrstttl > @JuddiW looks good, I would like to try if it works. What do you mean by syncing? You can have a look at this guide: https://gist.github.com/Chaser324/ce0505fbed06b947d962

Thank you for your help, I will try it.

So is the query feature merged?

[flschnepf]

Hello, I am also interested in this. Can someone make this function available?

[Damonyhk]

I‘ve implemented the query feature, but i don't konw how to sync it to github.

Hello, regarding xy2TrackST(x,y), can I obtain your code for it?

[markomiz]

@flschnepf @Damonyhk - This is a snippet of the code I use to do a similar thing (get s,t and the lane it belongs to)

the main function is the road.ref_line.match( point_.x, point_.y ); in the libOpenDRIVE code - then it is a bit more work to get the right sign of the t coordinate and the lane key.

I hope this helps

`
template
MapPoint
MapHelper::get_map_point_on_roads( const PointType &point_, bool search_all_roads, const std::setstd::string &road_ids_ ) const {

if( !search_all_roads && road_ids_.empty() ) {
    throw std::invalid_argument( "Road set is empty - can't find nearest point on an empty set of roads" );
}

double      t_coordinate    = std::numeric_limits<double>::max();
std::string closest_road_id = "";
double      closest_s       = 0;
odr::Vec3D  close_point{ 0.0, 0.0, 0.0 };

// Iterate over roads
for( const auto &road_pair : m_road_map.id_to_road ) {
    const auto &road = road_pair.second;

    if( !search_all_roads && road_ids_.count( road.id ) == 0 )
        continue;

    double     s             = road.ref_line.match( point_.x, point_.y );
    odr::Vec3D matched_point = road.ref_line.get_xyz( s );
    double     distance      = std::sqrt( pow( point_.x - matched_point[0], 2 ) + pow( point_.y - matched_point[1], 2 ) );
    if( distance < t_coordinate ) {
        t_coordinate    = distance;
        closest_road_id = road.id;
        closest_s       = s;
        close_point     = matched_point;
    }
}

// Check if the closest road ID was found
if( closest_road_id.empty() ) {
    std::cout << " point: " << point_.x << " " << point_.y << std::endl;
    throw std::runtime_error( "No closest road found." );
}

// Get the gradient/tangent vector at that point on the reference line
auto min_max_s = get_road_min_max_s( closest_road_id );
closest_s      = std::min( closest_s, min_max_s.second );
closest_s      = std::max( closest_s, min_max_s.first );

// Check if the road ID exists in the map
auto road_it = m_road_map.id_to_road.find( closest_road_id );
if( road_it == m_road_map.id_to_road.end() ) {
    throw std::runtime_error( "Closest road ID not found in the map." );
}

const auto &road = road_it->second;

odr::Vec3D grad = road.ref_line.get_grad( closest_s );

geometry::Point point_ahead( close_point[0] + grad[0], close_point[1] + grad[1] );
// Calculate the perpendicular angle of the gradient vector to calculate t
t_coordinate *= wind_left( geometry::Point( close_point[0], close_point[1] ), point_ahead, point_ );

// Check if the lanesection exists at closest_s
double lanesections0  = road.get_lanesection_s0( closest_s );
auto   lanesection_it = road.s_to_lanesection.find( lanesections0 );
if( lanesection_it == road.s_to_lanesection.end() ) {
    throw std::runtime_error( "Lanesection s0 not found in the road." );
}

const auto &lanesection = lanesection_it->second;

// Get the lane ID and check if it exists
int lane_id = lanesection.get_lane_id( closest_s, t_coordinate );
if( lane_id == 0 ) {
    lane_id = 1; // Prevent getting map location on center line
}
auto lane_it = lanesection.id_to_lane.find( lane_id );
if( lane_it == lanesection.id_to_lane.end() ) {
    throw std::runtime_error( "Lane ID not found in the lanesection." );
}

return MapPoint( odr::LaneKey( closest_road_id, lanesections0, lane_id ), closest_s, t_coordinate );

}`

[Damonyhk]

@markomiz
Thank you very much, your code is very useful to me. But I have a question, when I use the world coordinates of the start point of the reference line as the input of the match function, the result is always not 0, but a number close to 0.004. I am sure that my map offset is 0.

[markomiz]

@Damonyhk
If you look into the match function you see that it calls a golden_section_search function which is a numerical method that will not give an exact answer but one with a given tolerance.

[catmxt]

@markomiz Thank you for sharing this! Could you explain the get_road_min_max_s() function? I may be being naive, but shouldn't the minimum and maximum S-coordinate of a road be zero and the road's length respectively?

[JuddiW]

@catmxt please try this code.
struct road_st
{
std::string roadId;
int laneId;
std::string laneType;
bool inRoad;
double s;
double t;
double hdg;

road_st()
{
    roadId = "-1";
    laneId = 0xFF;
    laneType = "unknown";
    inRoad = false;
    s = 0;
    t = 0;
    hdg = 0;
}

};

road_st Road::xy2RoadST(double x,double y)
{
road_st st;
double closestS = 0;

try
{
    closestS = ref_line.match(x,y);
    double x1 = ref_line.get_xyz(closestS)[0];
    double y1 = ref_line.get_xyz(closestS)[1];
    double hdg = ref_line.get_geometry(closestS)->get_hdg(closestS);

    auto CrossProduct2D = [&](double x1, double y1, double x2, double y2) ->double
    {
        return x1 * y2 - x2 * y1;
    };

    auto DotProduct2D = [&](double x1, double y1, double x2, double y2) -> double
    {
        return x1 * x2 + y1 * y2;
    };

    auto PointToLineDistance2D =[&](double px, double py, double lx0, double ly0, double lx1, double ly1) -> double
    {
        double l0x      = lx1 - lx0;
        double l0y      = ly1 - ly0;
        double cp       = GetCrossProduct2D(lx1 - lx0, ly1 - ly0, px - lx0, py - ly0);
        double l0Length = sqrt(l0x * l0x + l0y * l0y);
        return cp / l0Length;
    };
    
    double t = PointToLineDistance2D(x,y,x1,y1,x1+cos(hdg),y1+sin(hdg));
    
    auto lanes = this->get_lanesection(closestS).get_lanes();
    std::sort(lanes.begin(),lanes.end(),[](const Lane &a, const Lane &b){return a.id < b.id;});

    double x2 = std::cos(hdg) * 100 + x1;
    double y2 = std::sin(hdg) * 100 + y1;
    double va_x = x2 - x1;
    double va_y = y2 - y1;
    double vb_x = x - x1;
    double vb_y = y - y1;

    double dp = DotProduct2D(va_x,va_y,vb_x,vb_y);
    double va_val = std::sqrt(va_x * va_x + va_y * va_y);
    double vb_val = std::sqrt(vb_x * vb_x + vb_y * vb_y);
    double theta = dp / (va_val * vb_val);

    if(theta < -1 && theta > -2)
        theta = -1;
    if(theta > 1 && theta < 2)
        theta = 1;

    double degrees = acos(theta) * 180 / M_PI;
    if(fabs(90 - degrees) > 0.005 || lanes.empty() || t < lanes.front().outer_border.get(closestS) || t > lanes.back().outer_border.get(closestS))
    {
        return st;
    }

    st.s = closestS;
    st.t = t;
    st.hdg = hdg;

    auto lane = get_lanesection(st.s).get_lane(st.s,st.t);
    st.laneId = lane.id;
    st.laneType = lane.type;
    st.roadId = id;
    return st;

}
catch(const std::exception& e)
{
    printf("%s\n",e.what());
    return road_st();
}    

}

[catmxt]

@JuddiW thank you! I was able to implement a version in my application which now gets the correct T-coordinate.