Hi @Xinyu Yang,
In your scenario, you have a vehicle trajectory defined in East-North-Up (ENU) coordinates, but your road data from OpenStreetMap is likely in a different coordinate system. The goal is to update the lane specifications for the ego vehicle without altering its reference point. So, what you need to do is to convert the road's geographic coordinates into ENU coordinates based on a defined origin. This will align the road data with your vehicle trajectory. Utilize functions like latlon2local to convert latitude and longitude from your road data to ENU coordinates. For more information on this function, please refer to
https://www.mathworks.com/help/driving/ref/latlon2local.html
Once both datasets are in ENU, you can proceed with updating the lane specifications as intended. Here is an updated code snippet incorporating these steps:
% Assuming you've already loaded gpsData and roadData from OSM % Define the reference point for transformation (e.g., from gpsData) geoReference = [gpsData.latitude(1), gpsData.longitude(1), gpsData.altitude(1)];
% Convert road centers from geographic to ENU coordinates
roadCentersENU = zeros(size(roadData, 1), 2);
for i = 1:size(roadData, 1)
[xEast, yNorth, ~] = latlon2local(roadData.RoadCenters{i}(:, 1), ...
roadData.RoadCenters{i}(:, 2), ...
roadData.RoadCenters{i}(1, 3), ...
geoReference);
roadCentersENU(i, :) = [xEast, yNorth];
end
% Update the RoadData with ENU coordinates
roadData.EastCoordinates = {roadCentersENU(:, 1)};
roadData.NorthCoordinates = {roadCentersENU(:, 2)};
% Now you can update lane specifications without changing ego vehicle reference egoRoadsWithUpdatedLanes = updateLaneSpec(laneDetections, egoRoadData, refLaneSpec, ... egoTrajectory, firstEgoWaypointLaneIdx, ... ReplicateUpdatedLaneWidth=true);
% Continue with your scenario simulation as before
So, the loop transforms each road's center coordinates from geographic format to ENU using latlon2local. This ensures that all spatial references are consistent. With both datasets in ENU format, you can now call updateLaneSpec without changing the reference point of the ego vehicle. For more information on this function, please refer to
After transforming coordinates, it is crucial to validate that both the road and vehicle trajectories visually align in your simulation environment. If issues arise during execution, use debugging tools or print statements to verify intermediate values (like transformed coordinates) for troubleshooting.
This approach will allow you to maintain the integrity of your ego vehicle's reference point while ensuring that all relevant data is accurately aligned for simulation purposes.
Hope this helps.
If you have further questions, please let me know.
