Techniques are disclosed to facilitate cooperative mapping for safe and efficient trajectory planning and collision avoidance by allowing nearby agents to share contextual information. The described techniques also function to extend the mapping range of a single agent by leveraging observations made by multiple agents. Furthermore, the techniques as described herein function to reduce uncertainty in trajectory planning by allowing agents to “see” behind occlusions, thus taking advantage of observations made by neighboring agents from different points of view. An efficient hardware implementation of the system is also presented that leverages the methodologies as discussed herein.