Я использую Map Builder ICP библиотеки MRPT для реализации 2D шлема на C ++, используя Sick LMS151...
при использовании графа поз для исправления замыкания контура, с использованием абсолютной позы...