Vasily, я в аналогичном проекте использовал Semi-Global Block Matching алгоритм, он был единственным, который работал очень хорошо. Здесь все зависит от калибровки камер, а именно от ректификации изображений с левой и правой камер. Это когда точки левого и правого изборажения имеют одинаковые y координаты. Я очень сильно намучался с этой калибровкой. Пришлось лепить целую конструкцию, чтобы камеры не смещались во время движения, иначе вся карта глубины превращалась в мусор и приходилось заново калибровать. Если потребуется могу прислать пример кода и параметров алгоритма.
http://www.cvlibs.net/software/libelas.html Еще посмотрите вот эту библиотеку, выдает очень хорошие результаты при большом расстоянии между камерами. Там же есть библиотека для одометрии и зд реконструкции, как раз что вам и нужно.
А вообще купите кинект, с ним все это гораздо проще.