BeS

Фактически то, что вам хочется найти - это клика во взвешенном подграфе. http://habrahabr.ru/post/154975/ вот в этой статье я описал один из способов поиска таких подграфов.

В классическом kinfu объем сцены ограничен кубом 3*3*3 метра, но есть разновидность kinfu LargeScale для произвольного объема сцены. Но насколько я помню нет имплементации kinfu на CPU, только на GPU. И вряд-ли мобильный робот имеет на борту GPU и достаточного размера аккумулятор чтобы все это переваривать дольше пары минут)

Для реконструкции можно попробовать использовать пары последовательных кадров в качестве стереопары(Через оптический поток вычислять расстояние между положениями камеры, а при помощи особых точек восстанавливать глубину точек сцены). Но для того, чтобы SLAM считался слэмом необходимо, чтобы он мог пройти тест с замыканием траекторий, а насколько я знаю эта задача до сих пор никем не решена для случая монокамеры...

http://habrahabr.ru/post/154975/ Вот моя статья на эту тему

В OpenCV в качестве точки берется центр региона.