Copy from https://github.com/bsdnoobz/opencv-code/blob/master/shape-detect.cpp