tree_matching function

3D matching of detected tree top positions with reference positions