SYSTEMS AND METHODS FOR DETECTING FLOOR FROM NOISY DEPTH MEASUREMENTS FOR ROBOTS
Systems and methods for detecting floor from noisy depth measurements for robots are disclosed herein. According to at least one non-limiting exemplary embodiment, a height map may be produced based on one or more depth measurements from a sensor of a robot. The height map may be utilized to determine surface normal vectors which may be further utilized by the robot to determine if regions of the height map are floor.
1 . A robotic system, comprising:
at least one sensor configured to generate a plurality of points corresponding to distance measurements;
a memory comprising plurality of computer readable instructions stored thereon; and
at least one controller configured to execute the plurality of computer readable instructions to:
receive a set of points from a scan by the at least one sensor;
project the set of points onto a two-dimensional height map, the height map comprises a plurality of pixels, each pixel being encoded with a height value based on height values of the set of points projected thereon;
calculate a surface normal unit vector for the each pixel of the height map based on the height values; and
determine one or more pixels of the height map corresponding to a floor space based on a respective surface normal unit vector being within a threshold deviation from a reference surface normal unit vector.
2 . The robotic system of claim 1 , wherein the at least one controller is further configured to execute the plurality of computer readable instructions to:
determine a first component of the respective surface normal unit vector for the each pixel of the height map based on height value differences between a first pixel and a second pixel, the second pixel being adjacent to the first pixel along a first axis;
determine a second component of the respective surface normal unit vector based on height value differences between the first pixel and a third pixel, the third pixel being adjacent to the first pixel along a second axis orthogonal to the first axis; and
calculate the respective surface normal unit vector based on the cross product of the first and second components.
3 . The robotic system of claim 2 , wherein the at least one controller is further configured to execute the plurality of computer readable instructions to:
determine a third component of the respective surface normal unit vector for each pixel of the height map based on height value differences between the first pixel and a fourth pixel, the fourth pixel being along the first axis and different from the second pixel;
determine a fourth component of the respective surface normal unit vector based on height value differences between the first pixel and a fifth pixel, the fifth pixel being along the second axis orthogonal to the first axis; and
calculate the respective surface normal unit vector based on an average of the cross product of the first and second components and the third and fourth components.
4 . The robotic system of claim 1 , wherein the controller is further configured to execute the plurality of computer readable instructions to,
produce the height map based on a subset of points from the set of points of the scan which are within a threshold distance from the robot.
5 . The robotic system of claim 1 , wherein the controller is further configured to execute the plurality of computer readable instructions to,
produce the height map based on a subset of points from the set of points which are within a threshold height above the robot or floor.
6 . The robotic system of claim 1 , wherein the at least one controller to is further configured to execute the plurality of computer readable instructions to:
produce a floor mask, the floor mask comprises a plurality of pixels identified as corresponding to floor, the pixels being pixels of at least one of: (i) a computer readable map, the computer readable map comprises objects localized thereon; or (ii) pixels of a depth image captured by the at least sensor.
7 . A non-transitory computer readable storage medium comprising a plurality of computer readable instructions stored thereon which, when executed by at least one controller, configure the at least one controller to:
receive a set of points from a scan by the at least one sensor;
project the set of points onto a two-dimensional height map, the height map comprises a plurality of pixels, each pixel being encoded with a height value based on height values of the points projected thereon;
calculate a surface normal unit vector for the each pixel of the height map based on the height values; and
determine one or more pixels of the height map corresponding to a floor space based on a respective surface normal unit vector being within a threshold deviation from a reference surface normal unit vector.
8 . The non-transitory computer readable storage medium of claim 7 , wherein the controller is further configured to execute the plurality of computer readable instructions to,
calculate the surface normal unit vector for each pixel by:
determine a first component of the respective surface normal unit vector for the each pixel of the height map based on height value differences between a first pixel and a second pixel, the second pixel being adjacent to the first pixel along a first axis;
determine a second component of the respective surface normal unit vector based on height value differences between the first pixel and a third pixel, the third pixel being adjacent to the first pixel along a second axis orthogonal to the first axis; and
calculate the respective surface normal unit vector based on the cross product of the first and second components.
9 . The non-transitory computer readable storage medium of claim 8 , wherein the controller is further configured to execute the plurality of computer readable instructions to,
determine a third component of the respective surface normal unit vector for each pixel of the height map based on height value differences between the first pixel and a fourth pixel, the fourth pixel being along the first axis and different from the second pixel;
determine a fourth component of the respective surface normal unit vector based on height value differences between the first pixel and a fifth pixel, the fifth pixel being along the second axis orthogonal to the first axis; and
calculate the respective surface normal unit vector based on an average of the cross product of the first and second components and the third and fourth components.
10 . The non-transitory computer readable storage medium of claim 7 , wherein the controller is further configured to execute the plurality of computer readable instructions to,
produce the height map based on a subset of points from the set of points of the scan which are within a threshold distance from the robot.
11 . The non-transitory computer readable storage medium of claim 7 , wherein,
the controller is further configured to execute the plurality of computer readable instructions to,
produce the height map based on a subset of points from the set of points which are within a threshold height above the robot or floor.
12 . The non-transitory computer readable storage medium of claim 7 , further comprising computer readable instructions which, when executed, cause the at least one controller to:
produce a floor mask, the floor mask comprises a plurality of pixels identified as corresponding to floor, the pixels being pixels of at least one of (i) a computer readable map, the computer readable map comprises objects localized thereon; or (ii) pixels of a depth image captured by the at least sensor.
13 . A method, comprising:
receiving a set of points from a scan by at least one sensor;
projecting the set of points onto a two-dimensional height map, the height map comprises a plurality of pixels, each pixel being encoded with a height value based on height values of the points projected thereon;
calculating a surface normal unit vector for the each pixel of the height map based on the height values; and
determining one or more pixels of the height map correspond to floor space based on the respective surface normal unit vector being within a threshold deviation from an reference surface normal unit vector.
14 . The method of claim 13 , further comprising:
calculating the surface normal unit vector for each pixel by:
determining a first component of the respective surface normal unit vector for each pixel of the height map based on height value differences between a first pixel and a second pixel, the second pixel being adjacent to the first pixel along a first axis;
determining a second component of the respective surface normal unit vector based on height value differences between the first pixel and a third pixel, the third pixel being adjacent to the first pixel along a second axis orthogonal to the first axis; and
calculating the respective surface normal unit vector based on the cross product of the first and second components.
15 . The method of claim 14 , further comprising:
determining a third component of the respective surface normal unit vector for the pixels of the height map based on height value differences between the first pixel and a fourth pixel, the fourth pixel being along the first axis and different from the second pixel;
determining a fourth component of the respective surface normal unit vector based on height value differences between the first pixel and a fifth pixel, the fifth pixel being along the second axis orthogonal to the first axis; and
calculating the respective surface normal unit vector based on an average of the cross product of the first and second components and the third and fourth components.
16 . The method of claim 13 , further comprising:
producing the height map based on a subset of points from the set of points of the scan which are within a threshold distance from the robot.
17 . The method of claim 13 , wherein,
producing the height map based on a subset of points from the set of points which are within a threshold height above the robot or floor.
18 . The method of claim 13 , further comprising the at least one controller:
producing a floor mask, the floor mask comprises a plurality of pixels identified as corresponding to floor, the pixels being pixels of at least one of (i) a computer readable map, the computer readable map comprises objects localized thereon; or (ii) pixels of a depth image captured by the at least sensor.
19 . A robotic system, comprising:
at least one sensor configured to generate a plurality points corresponding to distance measurements;
a non-transitory computer readable storage medium comprising a plurality of computer readable instructions stored thereon; and
at least one controller configured to execute the plurality of computer readable instructions to:
receive a set of points from a scan by the at least one sensor, the sensor includes a field of view, the field of view encompasses at least a floor and a flat object;
produce a height map based on the set of points, the height map comprises a plurality of pixels each comprising a respective height value;
detect pixels of the height map which correspond to floor based upon pixels of the height map comprising (i) a flat topology less than a threshold deviation, and (ii) most frequent height values of the pixels of the height map.