Semantics-based continuous-time motion Planner for autonomous vehicles using grid-based volumetric surface mapping. Semantically segments the Pointcloud into drivable and non-drivable areas and fuses segmented 3D point clouds into TSDF Voxels, and generates (ESDF) Euclidean Sign Distance values from TSDF which is employed by Planner for Collison and Road Following costs. The path is represented as multi-segment 3D polynomial function of time with its derivatives indicating position, velocity, and acceleration along the trajectory length and is formulated as a Quadratic Optimization problem minimizing collision, on-road, and path smoothness costs (minimizing position /velocity or acceleration over the segment length).
The Planner is evaluated on simulated scenes emulating basic on-road scenarios. The scene is geometrically represented using primitive 3D shapes and is observed using virtual cameras. Colored 3D pointcloud can be generated by projecting depth/RGB images into the 3D space using the camera poses. The map is built incrementally by fusing each set of pointcloud measurements into voxels using raycasting and calculating TSDF values for the respective voxels.
Note
Though the code is tested with the following simulated scenes, the algorithm can be used independently, providing its possible to generate the colored point cloud with semantic labels indicating road and obstacles. One suggested approach is to use RGB and Depth images and segment the RGB images to categorize points in the depth map.
A simple scenario of a single cylindrical obstacle in the road center.
A scenario with two obstacles placed along each other to evaluate planner turning through the obstacles.
Added a slope to the simple scenario to evaluate planning capability on sloped surfaces.
The code is tested on Ubuntu 18.04 and Ubuntu 20.04 with the specified library versions. A script to install these dependencies is provided in the scripts directory. Install using bash scripts/install_dependencies.sh.
- Eigen3 (3.3.7)
- Protobuf (2.6.1)
- Glog (0.4.0)
- Ceres (1.14.0)
- CMake (3.16.0)
- Generate TSDF layers
- Convert the TSDF layers to ESDF Layers
- Visualize the ESDF Map
- Planning using ESDF Map
Note
CMake Targets for the above specified programs are provided in the CMakeLists.txt
The pointcloud is generated by transforming the camera through the scene and incrementally integrating the point cloud and updating TSDF values for respective voxels.
The TSDF layers are saved at the path specified as an argument for both obstacles and drivable zone.
Usage: generate_tsdf <scene> <output_obstacles_layer> <output_drivable_layer> <output_pointcloud>
scene Scenario for mapping. Choose from the scenarios mentioned in the last step.
output_obstacles_layer Path to save the TSDF layer for driving zone
output_drivable_layer Path to save the TSDF layer for obstacles
output_pointcloud Path to save the colored point cloud as [XYZRGB] in .txt file.
For example: generate_tsdf tsdf_obstacles_layer.layer tsdf_free_layer.layer pointcloud.txt
Once we have a TSDF map for a scene, an ESDF map can be generated from it and saved at the path provided. Along with the TSDF layer input, the semantic category of the ESDF map has to pe specified. i.e whether the maps are for obstacles or drivable zone. For obstacles the road surface is masked (using the pointcloud semantic label) to prevent it to be considered as an obstacle.
Usage: esdf_from_tsdf <input_layer> <output_layer> <category>
input_layer Path to load the TSDF layer for driving zone
output_layer Path to load the TSDF layer for obstacles
category Category for obstacles. Valid values [obstacles, free]
For example: esdf_from_tsdf tsdf_obstacle_layer.layer esdf_obstacle_layer.layer obstacle
Here the esdf_from_tsdf is the generated executable.
ESDF Voxels can be visualized as heat-map colored point cloud from Red to Green indicating spectral variations in costs in decreasing order (not to be confused with the potential function which is calculated as squared and linear function of these values).
Usage: visualzie_esdf_voxels <input_layer_path> <layer_category>
Where <input_layer_path> is the path of the TSDF layer generated in the last step and <layer_category> indicates the semantic category of the map. i.e obstacles or free space.
For example to save ESDF map for obstacles use :
visualzie_esdf_voxels esdf_free_layer.layer free and
similarly for drivable zone use :
visualzie_esdf_voxels esdf_obstacle_layer.layer obstacles
Note
A colored pointcloud (XYZRGB) with voxel centers is saved as layer_name_pointcloud.txt (e.g esdf_obstacles_layer_pointcloud.txt) which can be view directly in Meshlab.
After constructing ESDF maps for both obstacles and drivable space, the ESDF distance information is employed to calculate potential fields for collision and offroad penalties which allows formulating the path planning as an optimization problem over the space of polynomial trajectories.
Usage: semantic_planner <start> <goal> <obstacles_layer_path> <drivable_layer_path> <pointcloud_path>
start Start position as 3D comma separated value
goal Goal Positionn as 3D comma separated value
obstacles_layer_path ESDF map path generated using esdf_from_tsdf program. Used to get distance to obstacles along the prospective paths
drivable_layer_path ESDF map path generated using esdf_from_tsdf program for free space
pointcloud_path Pointcloud path over which the planned path is highlighted
For example: semantic_planner 0.0,0.0,0.0 5.0,5.,6.0 esdf_obstacle_layer.layer esdf_freer.layer pointcloud.txt
Saves the planned path as colored pointcloud [XYZRGB] with highlighted planned path at the path provided as an argument with name ${pointcloud_name}_plan.txt
Below you can find scripts to build and perform all the steps defined in the above steps.
A script to compile the code using CMake is provided in the scripts directory.
bash scripts/compile.sh
A Script is provided to automate the aforementioned steps and save the results in the output folder(contents described in the following section). The planned path (highlighted over scene) is saved as a colored pointcloud [XYZRGB format] in the output directory along with ESDF colored pointclouds for both obstacles and drivable zone.
bash scripts/run_scenario.sh <scenario>
The valid values for the scenario are simple, multi, and slope.
For each scenario, the following output files are generated.
- output/pointcloud_scenario.txt where scenario can be [simple, multi, slope]
- Scene as colored pointcloud
- output/esdf_obstacles_layer_pointcloud.txt
- ESDF layer for obstacles saved as a colored heatmap point cloud
- output/esdf_free_layer_pointcloud.txt
- ESDF layer for drivable space saved as a colored heatmap point cloud
- output/pointcloud_scenario_plan.txt
- Planned path highlighted over the scene point cloud
A document describing the Planner equations and derivations can be found as planner.pdf in the main directory.
Fix (shown in below image) Use multiple views from the camera to reduce blind spots particularly at the corners of obstacles which usually have empty ESDF Voxels because of obstacle blocking rays from the camera.
Sometimes Planner still goes through the obstacles as a compromise between smoothness costs and collision costs as it picks a colliding path over a non-smooth collision-free path as shown below.
Fix The weight for smoothness costs can be further tweaked to find a suitable compromise between the costs to prioritize collision-free paths over smooth paths. Another more practical approach is to do local replanning along with using a global Planner.
The Planner has difficulty planning through large holes and in unexplored areas specifically because is poised to behave conservatively thus considering unknown space an obstacle and non-traversable. The issue can be mitigated by having a certain limited region around the vehicle to be considered free so that the vehicle can start planning when having no initial knowledge of the surroundings.
Another option can be to do an initial pass over the environment in an offline step to have an initial estimate of the map.
The mapping implementation is based on Voxblox, particularly it is extended to include semantic segmentation for each voxel. The detailed algorithm for constructing ESDF values directly from TSDF values is discussed in the following paper.
Helen Oleynikova, Zachary Taylor, Marius Fehr, Juan Nieto, and Roland Siegwart, “Voxblox: Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning”, in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2017.
For the planning, Voxblox Planner is extended to include semantic information for drivable space to constrain the planned path to valid on-road regions.
Helen Oleynikova, Michael Burri, Zachary Taylor, Juan Nieto, Roland Siegwart, and Enric Galceran, “Continuous-Time Trajectory Optimization for Online UAV Replanning”. In IEEE Int. Conf. on Intelligent Robots and Systems (IROS), October 2016.
[pdf | bibtex | video]