Implemented matching function between gt and point cloud