This function performs the computationally expensive step of matching two rectified and undistorted stereo images. The output is a dense disparity map. If camera parameters are known, this allows for three dimensional reconstruction.

that is, for a 3x3 summing window, the filtering window must be 5x5 to implement the algorithm correclty (see http://en.wikipedia.org/wiki/Summed_area_table#The_algorithm and note that points A, B and C are outside the summing window)

I tested this with a simple matrix for validation. Otherwise, thanks for the useful code.