SYSTEMS AND METHODS FOR SENSOR REGISTRATION BASED ON FLOOR ALIGNMENT

1 . A method of sensor registration for a workcell including a floor, the method comprising:

disposing a plurality of three dimensional (3D) image sensors proximate to the workcell;

acquiring, with the plurality of 3D image sensors, a plurality of 3D images of the workcell, wherein at least one of the plurality of 3D images includes at least a portion of the floor;

determining, based on a first user selection of a region of a first image of the plurality of 3D images of the workcell, a first floor point indicating a first portion of the floor in the first image;

computing a first floor plane representing the floor based on the first floor point; and

registering the 3D image sensors to each other and to the workcell based at least in part on the plurality of 3D images and the first floor plane.

2 . The method of claim 1 , wherein computing the first floor plane comprises:

determining, within the first image, a region-of-interest around the first floor point, wherein the region-of-interest includes a plurality of candidate points within the region of interest in the first image;

computing a fitted plane based on at least one of the plurality of candidate points of the region-of-interest; and

determining whether the fitted plane is suitable to be adopted as the first floor plane to register a first 3D image sensor acquiring the first image.

3 . The method of claim 2 , wherein the fitted plane minimizes a sum of square distances from the plurality of candidate points to the fitted plane.

4 . The method of claim 2 , wherein determining whether the fitted plane is suitable to be adopted as the first floor plane to register the first 3D image sensor comprises:

computing a mean normal direction of the fitted plane based on a mean of normal directions of the plurality of candidate points;

computing a variance of the normal directions of the plurality of candidate points; and

comparing the variance to a predetermined threshold to determine whether the fitted plane is suitable to be adopted as the first floor plane to register the first 3D image sensor.

5 . The method of claim 4 , wherein computing the first floor plane further comprises:

when the variance is below the predetermined threshold,

adopting the fitted plane as the first floor plane;

computing a transformation that aligns the mean normal direction to a normal direction of the floor; and

applying the transformation to an entire point cloud of the first 3D image sensor to register the first 3D image sensor.

6 . The method of claim 4 , wherein computing the first floor plane further comprises:

when the variance is above the predetermined threshold,

rejecting the first user selection; and

requesting another user selection of another region of the first image for floor alignment.

7 . The method of claim 2 , wherein determining the region-of-interest comprises:

determining a ray line extending from a point of view to a selected point of the first user selection in the first image;

identifying the region-of-interest based on a first radius around the ray line;

accepting the region-of-interest to compute the fitted plane for floor alignment when a quantity of the plurality of candidate points in the region-of-interest is more than a predetermined threshold; and

when the quantity of the plurality of candidate points in the region-of-interest is less than the predetermined threshold,

rejecting the first user selection, and

requesting another user selection of another region of the first image for floor alignment.

8 . The method of claim 2 , wherein determining the region-of-interest comprises:

determining, based on additional user selections, additional floor points on the floor in the first image; and

identifying the region-of-interest based on the first floor point and the additional floor points.

9 . The method of claim 2 , wherein computing the fitted plane comprises:

determining candidate planes each containing at least one candidate point of the plurality of candidate points and two additional points in the first image, wherein each point in the first image is considered an inlier point passed by a candidate plane when the point is within a same predetermined distance from the candidate plane;

selecting a plane from the candidate planes, wherein the selected plane passes the most inlier points in the first image among the candidate planes; and

computing the fitted plane based on the inlier points passed by the selected plane, wherein the fitted plane minimizes a sum of square distances from the inlier points to the fitted plane.

10 . The method of claim 1 , further comprising:

determining, based on a second user selection of a second region of a second image of the plurality of 3D images of the workcell, a second floor point indicating a second portion of the floor in the second image;

computing a second floor plane representing the floor based on the second floor point; and

determining that the first floor plane is aligned with the second floor plane to register the 3D image sensors acquiring the first image and the second image, wherein the plurality of 3D image sensors are physically separated from each other by at least one meter and configured to acquire 3D images independently from each other.

11 . The method of claim 1 , further comprising:

adjusting a vertical offset of at least one 3D image sensor of the plurality of 3D image sensors based on a user input in view of at least one image of the workcell acquired by the at least one 3D image sensor; and

determining, based on the adjusting, that a floor in the at least one image has a vertical position aligned to a floor level in a global frame of the workcell.

12 . A control system for sensor registration for a workcell including a floor, the system comprising:

a plurality of three dimensional (3D) image sensors proximate to the workcell; and

a controller configured to:

cause the plurality of 3D image sensors to acquire a plurality of 3D images of the workcell, wherein at least one of the plurality of 3D images includes at least a portion of the floor,

determine, based on a first user selection of a region of a first image of the plurality of 3D images of the workcell, a first floor point indicating a first portion of the floor in the first image,

compute a first floor plane representing the floor based on the first floor point, and

register the 3D image sensors to each other and to the workcell based at least in part on the plurality of 3D images and the first floor plane.

13 . The control system of claim 12 , wherein the controller is further configured to:

determine, within the first image, a region-of-interest around the first floor point, wherein the region-of-interest includes a plurality of candidate points in the first image;

compute a fitted plane based on at least one of the plurality of candidate points of the region-of-interest; and

determine whether the fitted plane is suitable to be adopted as the first floor plane to register a first 3D image sensor acquiring the first image.

14 . The control system of claim 13 , wherein the controller is further configured to:

compute a mean normal direction of the fitted plane based on a mean of normal directions of the plurality of candidate points;

compute a variance of the normal directions of the plurality of candidate points; and

compare the variance to a predetermined threshold to determine whether the fitted plane is suitable to be adopted as the first floor plane to register the first 3D image sensor.

15 . The control system of claim 14 , wherein the controller is further configured to:

when the variance is below the predetermined threshold,

adopt the fitted plane as the first floor plane,

compute a transformation that aligns the mean normal direction to a normal direction of the floor, and

apply the transformation to an entire point cloud of the first 3D image sensor to register the first 3D image sensor; and

when the variance is above the predetermined threshold,

reject the first user selection, and

request another user selection of another region of the first image for floor alignment.

16 . The control system of claim 13 , wherein the region-of-interest is determined based on:

determining a ray line extending from a point of view to a selected point of the first user selection in the first image;

identifying the region-of-interest based on a first radius around the ray line;

accepting the region-of-interest to compute the fitted plane for floor alignment when a quantity of the plurality of candidate points in the region-of-interest is more than a predetermined threshold; and

when the quantity of the plurality of candidate points in the region-of-interest is less than the predetermined threshold,

rejecting the first user selection, and

requesting another user selection of another region of the first image for floor alignment.

17 . The control system of claim 13 , wherein the region-of-interest is determined based on:

determining, based on additional user selections, additional floor points on the floor in the first image; and

identifying the region-of-interest based on the first floor point and the additional floor points.

18 . The control system of claim 13 , wherein the controller is further configured to:

determine candidate planes each containing at least one candidate point of the plurality of candidate points and two additional points in the first image, wherein each point in the first image is considered an inlier point passed by a candidate plane when the point is within a same predetermined distance from the candidate plane;

select a plane from the candidate planes, wherein the selected plane passes the most inlier points in the first image among the candidate planes; and

compute the fitted plane based on the inlier points passed by the selected plane, wherein the fitted plane minimizes a sum of square distances from the inlier points to the fitted plane.

19 . The control system of claim 12 , wherein the controller is further configured to:

determine, based on a second user selection of a second region of a second image of the plurality of 3D images of the workcell, a second floor point indicating a second portion of the floor in the second image;

compute a second floor plane representing the floor based on the second floor point; and

determine that the first floor plane is aligned with the second floor plane to register the 3D image sensors acquiring the first image and the second image, wherein the plurality of 3D image sensors are physically separated from each other by at least one meter and configured to acquire 3D images independently from each other.

20 . The control system of claim 12 , wherein the controller is further configured to:

adjust a vertical offset of at least one 3D image sensor of the plurality of 3D image sensors based on a user input in view of at least one image of the workcell acquired by the at least one 3D image sensor; and

determine, based on the adjusting, that a floor in the at least one image has a vertical position aligned to a floor level in a global frame of the workcell.