#include <cmath>
#include <queue>
#include <algorithm>
#include "opencv2/core/core.hpp"
#include "fast_marching_inl.hpp"