Method for efficient volumetric integration for 3D sensors

1. A method of mapping an environment, comprising:

computing a plurality of radial components and a plurality of angular components for a positive inverse sensor model (ISM) of an occupancy grid;

obtaining a detection at a sensor from an object in an environment surrounding a vehicle;

selecting a radial component corresponding to a range of the detection from the plurality of radial components and selecting an angular component corresponding to an angle of the detection from the plurality of angular components;

multiplying the selected radial component and the selected angular component to create an occupancy grid for the detection; and

mapping the environment using the occupancy grid.

2. The method of claim 1 , wherein the ISM further comprises a negative ISM and a probability value of the negative ISM is assigned using a quad-tree structure of the occupancy grid.

3. The method of claim 2 , wherein a node of the quad-tree structure corresponds to a solid angle and a first level of the quad-tree structure includes solid angle bins that combine to cover a field of view of the sensor.

4. The method of claim 3 , further comprising partitioning a solid angle bin at a level of the quad-tree structure into a plurality of sub solid angles at a sub level of the quad-tree structure when the solid angle bin at the level is includes the detection.

5. The method of claim 4 , further comprising assigning a probability value to a solid angle bin for which there is no detection, wherein the probability value reflects an absence of the detection in the solid angle bin.

6. The method of claim 2 , further comprising combining the probability values from the negative ISM with a probability value from the positive ISM to create the occupancy grid.

7. The method of claim 1 , further comprising navigating the vehicle with respect to the object based on the mapping of the environment.

8. A system for mapping an environment at a vehicle, comprising:

a sensor configured to obtain a detection from an object in an environment surrounding the vehicle; and

a processor configured to:

compute a plurality of radial components and a plurality of angular components for a positive inverse sensor model (ISM) of an occupancy grid;

select a radial component corresponding to a range of the detection from the plurality of radial components and selecting an angular component corresponding to an angle of the detection from the plurality of angular components;

multiply the selected radial component and the selected angular component to create an occupancy grid for the detection; and

map the environment using the occupancy grid.

9. The system of claim 8 , wherein the processor is further configured to assign a probability value of a negative ISM of the occupancy grid using a quad-tree structure of the occupancy grid.

10. The system of claim 9 , wherein a node of the quad-tree structure corresponds to a solid angle and a first level of the quad-tree structure includes solid angle bins that combine to cover a field of view of the sensor.

11. The system of claim 10 , wherein the processor is further configured to partition a solid angle bin at a level of the quad-tree structure into a plurality of sub solid angles at a sub level of the quad-tree structure when the solid angle bin at the level includes the detection.

12. The system of claim 11 , wherein the processor is further configured to assign a probability value to a solid angle bin for which there is no detection, wherein the probability value reflects an absence of the detection at the solid angle bin.

13. The system of claim 9 , wherein the processor is further configured to combine the probability values from the negative ISM with a probability value from a positive ISM to create the occupancy grid.

14. The system of claim 8 , wherein the processor is further configured to navigate the vehicle with respect to the object based on the mapping of the environment.

15. A vehicle, comprising:

a sensor configured to obtain a detection from an object in an environment surrounding the vehicle; and

a processor configured to:

compute a plurality of radial components and a plurality of angular components for a positive inverse sensor model (ISM) of an occupancy grid;

select a radial component corresponding to a range of the detection from the plurality of radial components and selecting an angular component corresponding to an angle of the detection from the plurality of angular components;

multiply the selected radial component and the selected angular component to create an occupancy grid for the detection; and

map the environment using the occupancy grid.

16. The vehicle of claim 15 , wherein the processor is further configured to assign a probability value to a negative ISM of the occupancy grid using a quad-tree structure of the occupancy grid.

17. The vehicle of claim 16 , wherein a node of the quad-tree structure corresponds to a solid angle and a first level of the quad-tree structure includes solid angle bins that combine to cover a field of view of the sensor.

18. The vehicle of claim 17 , wherein the processor is further configured to partition a solid angle bin at a level of the quad-tree structure into a plurality of sub solid angles at a sub level of the quad-tree structure when the solid angle bin at the level includes the detection.

19. The vehicle of claim 18 , wherein the processor is further configured to assign a probability value to a solid angle bin for which there is no detection, wherein the probability value reflects an absence of the detection at the solid angle bin.

20. The vehicle of claim 16 , wherein the processor is further configured to combine the probability values from the negative ISM with a probability value from a positive ISM to create the occupancy grid.