Method and system for generating multidimensional maps of a scene using a plurality of sensors of various types

A system and method for generating a high-density three-dimensional (3D) map are disclosed. The system comprises acquiring at least one high density image of a scene using at least one passive sensor; acquiring at least one new set of distance measurements of the scene using at least one active sensor; acquiring a previously generated 3D map of the scene comprising a previous set of distance measurements; merging the at least one new set of distance measurements with the previous set of upsampled distance measurements, wherein merging the at least one new set of distance measurements further includes accounting for a motion transformation between a previous high-density image frame and the acquired high density image and the acquired distance measurements; and overlaying the new set of distance measurements on the high-density image via an upsampling interpolation, creating an output 3D map.