Takes track images output by SLAM and outputs points along the center-line in a csv file.
Get track image and info files from Cartographer or some other SLAM tool. The image file should be in pgm format and the info file should be in yaml format. Here is an example track pgm and yaml combination:
image: track.pgm
resolution: 0.050000
origin: [-5.659398, -4.766974, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
This center-line-fitting algorithm uses four steps:
- Denoise
- Skeletonize
- Prune and Order
- Subsample
In this step, we run the DBSCAN clustering algorithm on the image twice. The first time, we run it on the positive space and keep only the pixels from the largest cluster. The second time, we run it on the negative space and keep only the pixels from the two largest clusters.
Next we run Zhang's Skeletonize algorithm on the image. Here is the output of "Skeletonize" overlayed over the denoised image:
Next we use a Depth First Search to try to find a large cycle in graph defined by adjacent pixels in the image. We remove all pixels that do not fall along the cycle. Not only does pruning remove extra branches in the skeleton, it gives us an ordering of the pixels that goes around the track. Here is the output of "Pruning" overlayed over the denoised image:
Finally, we subsample pixels along the cycle we came up with. Basically, if the x is the subsample period, we keep every xth pixel on the cycle. The result with subsample period 6 looks like this:
Clone the repository and run the center-line-fitting script:
git clone https://github.com/mlab-upenn/center-line-fitting.git
cd center-line-fitting
python src/fit_centerline.py
To control the plots that are displayed, use
python center-line-fitting.py --plot_mode <0, 1, or 2>
0 shows no plots, 1 (default) shows basic plots, and 2 shows all plots
You can specify the input file paths and output directory using the command line:
python center-line-fitting.py --pgm_path <path to track image> --yaml_path <path to track info> --out_dir <path to output directory>
You can change the subsampling period as follows:
python center-line-fitting.py --subsample_period 20
This changes how sparesly the points are sampled from the center-line path
