Create Egocentric Occupancy Map from Driving Scenario Designer
This example shows how to create an egocentric occupancy map from the Driving Scenario Designer app. This example uses obstacle information from the vision detection generator to update the egocentric occupancy map.
Gets obstacle information and road geometry using vision detection generator.
Creates an ego centric occupancy map using binary occupancy map.
Updates the ego centric occupancy map using lane boundaries and obstacle information.
Automated driving systems use multiple on-board sensors on the ego vehicle like radar, cameras, and lidar. These sensors are used to perceive information from the surroundings and environment. It is important to collate information from these heterogeneous sensors into a common temporal frame of reference. This is usually done using an egocentric occupancy map. This map contains information about the surrounding environment like road geometry, free space, and obstacles. This egocentric occupancy map is used by planning algorithms for navigation. The ego vehicle can respond to dynamic changes in the environment by periodically updating the information in this ego centric occupancy map.
This example shows how to use lane and obstacle information obtained from a scenario to create and update an ego centric occupancy map.
This example also uses a straight road scenario designed using Driving Scenario Designer (DSD). For more details, see Driving Scenario Designer (Automated Driving Toolbox). You can also create a Driving Scenario programmatically. For more details, refer to Create Driving Scenario Programmatically (Automated Driving Toolbox).
Get Lane Boundaries and Obstacle Information from Driving Scenario
The scenario used in this example is a straight road with four lanes. This scenario has one ego vehicle and six target vehicles which follow their respective predefined paths. Create a scenario using the helper function,
The scenario used in this example is shown in the following figure.
[scenario, egoVehicle] = exampleHelperCreateStraightRoadScenario;
This example uses a vision detection generator which synthesizes camera sensor mounted at the front of the ego vehicle in the driving scenario. This generator is configured to detect lane boundaries and obstacles.
For more details, see
visionDetectionGenerator (Automated Driving Toolbox).
The update interval of the vision detection generator is configured to generate detections at 0.1 second intervals, which is consistent with the update rate of typical automotive vision sensors.
actorProfiles for the scenario, which contain physical and radar profiles of all driving scenario actors including the ego vehicle. Specify those actors and other configuration parameters for the vision detection generator.
profiles = actorProfiles(scenario); % Configure vision detection generator to detect lanes and obstacles sensor = visionDetectionGenerator('SensorIndex', 1, ... 'SensorLocation', [1.9 0], ... 'DetectionProbability', 1, ... 'MinObjectImageSize', [5 5], ... 'FalsePositivesPerImage', 0, ... 'DetectorOutput', 'Lanes and objects', ... 'Intrinsics', cameraIntrinsics([700 1814],[320 240],[480 640]), ... 'ActorProfiles', profiles,... 'UpdateInterval', 0.1);
To get detections, the
sensor object is called for each simulation step. This
sensor object call occurs in the helper function
Create an Egocentric Occupancy Map
This example uses a
binaryOccupancyMap object to create an egocentric occupancy map.
Create a square occupancy map with 100 meters per side and a resolution of 2 cells per meter. Set all the cells to occupied state by default.
egoMap = binaryOccupancyMap(100, 100, 2); setOccupancy(egoMap,ones(200, 200));
By default, the map origin is at bottom-left. Move the
egoMap origin to the center of the occupancy map. This converts the occupancy map to an egocentric occupancy map.
egoMap.GridOriginInLocal = [-egoMap.XLocalLimits(2)/2, ... -egoMap.YLocalLimits(2)/2];
Update the Egocentric Occupancy Map with Obstacle Information
Before updating the
egoMap, initialize the visualization window. Obstacles in the
egoMap visualization are represented as black (occupied) and free space as white (unoccupied).
hAxes = exampleHelperSetupVisualization(scenario);
Setup a loop for executing the scenario using
advance(scenario). This loop should move the
egoVehicle along the road, updating the pose as it moves.
move on the
egoMap using the updated pose to get updated detections. Clear the map of all obstacles for each update.
% Advancing the scenario while advance(scenario) % Ego vehicle position egoPose = egoVehicle.Position; egoYaw = deg2rad(egoVehicle.Yaw); % Move the origin of grid to the face of the ego vehicle. move(egoMap, [egoPose(1), egoPose(2)]); %Reset the egoMap before updating with obstacle information setOccupancy(egoMap, ones(egoMap.GridSize));
Lane boundaries and obstacles information generated from a vision detection generator are used to find occupied space that needs to be updated in the
This example uses road boundaries information extracted from the lane boundaries. The region outside road boundaries is also considered occupied.
exampleHelperGetObstacleDataFromSensor helper function extracts road boundaries and obstacle information generated from the vision detection generator.
[obstacleInfo, roadBorders, isValidLaneTime] = ... exampleHelperGetObstacleDataFromSensor(scenario, egoMap, ... egoVehicle, sensor);
Depending upon the range of the vision detection generator, the detections may fall outside the
egoMap bounds. Using the
egoMap bounds, the
exampleHelperFitlerObstacles function extracts the obstacles and free space that are within the egocentric map.
[obstaclePoints, unoccupiedSpace] = exampleHelperFilterObstacles(... egoMap, obstacleInfo, ... roadBorders, ... isValidLaneTime, egoVehicle);
Use the filtered obstacle and free space locations to update the egocentric map. Update the visualization.
% Set the occupancy of free space to 0 if ~isempty(unoccupiedSpace) setOccupancy(egoMap, unoccupiedSpace, 0); end % Set the occupancy of occupied space to 1 if ~isempty(obstaclePoints) setOccupancy(egoMap, obstaclePoints, 1); end % Updating the visualization exampleHelperUpdateVisualization(hAxes, egoVehicle, egoPose, egoYaw, egoMap); end