This project has been developed as an implementation of a SLAM technique called GraphSLAM. This technique applies the theory of graphs to create an on-line optimization system that allows robots to map the scenario and locate themselves using a Time of Flight camera as the input source. To do that, a RGB-D system has been calibrated and used to create color 3D point clouds. With this information, the feature detector module estimates, as a first approximation, the pose of the camera. Therefore, a ICP pose refinement completes the graph structure. Finally, a HogMAN graph optimizer close the loop on each iteration using a hierarchical manifold optimization. As a result, 3D color maps are created containing, at the same time, the exact position of the robot over the map. ____________________________________________________________________________________________________________________________________