C++ (OpenCV) implementation of the adaptive manifold high dimensional filter