A method for mapping an environment comprises moving a sensor along a path from a start location (P0) through the environment, the sensor generating a sequence of images, each image associated with a respective estimated sensor location and comprising a point cloud having a plurality of vertices, each vertex comprising an (x,y,z)-tuple and image information for the tuple. The sequence of estimated sensor locations is sampled to provide a pose graph (P) comprising a linked sequence of nodes, each corresponding to a respective estimated sensor location. For each node of the pose graph (P), a respective cloud slice (C) comprising at least of portion of the point cloud for the sampled sensor location is acquired. A drift between an actual sensor location (Pi+1) and an estimated sensor location (Pi) on the path is determined. A corrected pose graph (P′) indicating a required transformation for each node of the pose graph (P) between the actual sensor location (Pi+1) and the start location (P0) to compensate for the determined drift is provided. The sequence of estimated sensor locations is sampled to provide a deformation graph (N) comprising a linked sequence of nodes, each corresponding to respective estimated sensor locations along the path. For at least a plurality of the vertices in the cloud slices, a closest set of K deformation graph nodes is identified and a respective blending function based on the respective distances of the identified graph nodes to a vertex is determined. Transformation coefficients for each node of the deformation graph are determined as a function of the 20 required transformation for each node of the pose graph (P) to compensate for the determined drift. Tuple coordinates for a vertex are transformed to compensate for sensor drift as a function of the blending function and the transformation coefficients for the K deformation graph nodes closest to the vertex.