Semi Global Matching Matlab Code