An imaging system for localization and mapping of a scene including static and dynamic objects. A sensor acquires a sequence of frames in motion or stationary. A memory to store a static map of static objects and an object map of each dynamic object in the scene. The static map includes a set of landmarks, and the object map includes a set of landmarks and a set of segments. A localizer registers keypoints of the frame with landmarks in the static map using frame-based registration and to register some segments in the frame with segments in the object map using a segment-based registration. A mapper to update each object map with keypoints forming each segment and keypoints registered with the corresponding object map according to the segment-based registration, and to update the static map with the remaining keypoints in the frame using the keypoints registered with the static map.