Radu, I would be glad to help, only I am a real beginner when it comes to mapping in 3D. I used the last week getting to know Octomap a bit. I started to explore your sources (mainly scan_graph.cpp). Is there any particular point I should have a look at? Kind regards Raph -- View this message in context: http://ros-users.122217.n3.nabble.com/Mapping3d-tp2205074p2272233.html Sent from the ROS-Users mailing list archive at Nabble.com.