Mapping an environment using a state of a robotic device

A mapping system and method for a robotic device 360 are presented, along with the method itself. The robotic device comprises an image capture device 320 such as a camera moveably mounted thereon, a state engine 340 which monitors the pose of the image capture device, and a mapping engine 350 which creates a 3D model 370 of the environment surrounding the robot. The pose of the image capture device with respect to the model is determined using an odometric error, a kinematic error, and a geometric error. The odometric error relates to the movement of the robotic device, the kinematic error relates to the movement of the imaging device relative to the robot, and the geometric error relates to the change in the image of the model. The robotic device comprises actuators 332 for controlling movement relative to the environment and may comprise actuators 334 to move the imaging device relative to the robot. The mapping engine may construct a deformation graph to apply a deformation to the model and may segment the model into active and inactive portions.