Восстановление 3д изображения по двум стереоснимкам

Есть 2 стереоснимка, мне по ним нужно получить облако точек (т.е. 3д изображение). Алгоритм для этого нашел, но вот как правильно это сделать, в упор не понимаю, возможно где то ошибка... каждый раз получаю ошибки в изображении. облако точек строю с помощью матлаб.

вот и алгоритм https://courses.cs.washington.edu/courses/cse455/09wi/Lects/lect16.pdf