Visual robot pose estimation
A method for estimating a pose of a robot using two co-planar points on an object plane in the environment and two co-planar lines on the object plane, includes capturing with a camera on the robot an image of the object in the environment, including two observed points corresponding to the two co-planar points on the object and two observed lines corresponding to the two co-planar lines on the object; projecting onto the image plane the two co-planar points to obtain two projected points and projecting the two co-planar lines to obtain two projected lines; determining a point projection error by comparing the two projected co-planar points to corresponding observed points; determining a line projection error by comparing the two projected co-planar lines to corresponding observed lines; and estimating a current robot pose if the point projection error and the line projection error are below a predetermined error threshold.
1. A method for estimating a pose of a robot traveling in an environment using two co-planar points (P 1 , P 2 ) on an object plane on an object in the environment and two co-planar lines (P 1 -A, P 2 -B) on the object plane on the object, the method comprising
capturing with a camera mounted on the robot images of the environment, including images of the object in the environment;
receiving camera images from the camera by a video decoder, including a camera image of the object;
decoding by the video decoder the received camera images into decoded digital images, including a decoded digital image of the object;
identifying, using a processor and memory, on the decoded digital image of the object two observed points (op 1 , op 2 ) corresponding to the two co-planar points (P 1 , P 2 ) on the object;
identifying, using the processor and memory, on the decoded digital image of the object two observed image lines (op 1 - a , op 2 - b ) corresponding to the two co-planar lines (P 1 -A, P 2 -B) on the object;
projecting, using the processor and memory, onto the image plane of the camera the two co-planar points (P 1 , P 2 ) on the object plane to obtain two projected co-planar image points (pp 1 , pp 2 ) and projecting, using the processor and memory, the two co-planar lines (P 1 -A, P 2 -B) on the object plane to obtain two projected co-planar image lines (pp 1 - a , pp 2 - b );
determining, using the processor and memory, a point projection error by comparing the two projected co-planar image points (pp 1 , pp 2 ) to corresponding observed points (op 1 , op 2 );
determining, using the processor and memory, a line projection error by comparing the two projected co-planar image lines (pp 1 - a , pp 2 - b ) to corresponding observed image lines (op 1 - a , op 2 - b ); and
estimating, using the processor and memory, a current robot pose if the point projection error and the line projection error are below a predetermined error threshold.
2. The method of claim 1 further including combining, using the processor and memory, the point projection error and the line projection error to establish a combined error, E total , and comparing the E total to the predetermined error threshold.
3. The method of claim 2 further including performing, using the processor and memory, a non-linear optimization on the two observed points (op 1 , op 2 ) relative to the two projected points (pp 1 , pp 2 ) and the two observed image lines (op 1 - a , op 2 - b ) and relative to the two projected co-planar image points (pp 1 , pp 2 ) before determining the combined error, E total .
4. The method of claim 3 wherein if the combined error, E total , is not less than the error threshold, the method further includes performing, using the processor and memory, a non-linear optimization on the two observed points (op 1 , op 2 ) relative to the two projected points (pp 1 , pp 2 ) and the two observed image lines (op 1 - a , op 2 - b ) and re-determining the combined error, E total .
5. The method of claim 2 wherein the point projection error is determined, using the processor and memory, according to the following formula:
E
points
=
∑
i
q
i
-
s
i
wherein q i are the projected points on image plane and s i are the observed points on the image plane.
6. The method of claim 5 wherein the line projection error is determined, using the processor and memory, according the following formula:
E
lines
=
n
ae
→
-
n
ae
′
→
+
n
bf
→
-
n
bf
′
→
wherein n ae and n′ ae are the projected lines on image plane and n bf and n′ bf are the observed lines on the image plane.
7. The method of claim 6 wherein the combined point projection error and line projection error is determined, using the processor and memory, according the following formula:
E
total
=
w
points
E
points
+
w
lines
E
lines
=
w
points
∑
i
q
i
-
s
i
+
w
lines
(
n
ae
→
-
n
ae
′
→
+
n
bf
→
-
n
bf
′
→
)
wherein W points and W lines are the weighting factors to balance the two kinds of errors.
8. The method of claim 1 wherein the environment is a warehouse.
9. The method of claim 8 wherein the object is an electric charging station for the robot in the warehouse.
10. A robot traveling in an environment, estimating a robot pose using two co-planar points (P 1 , P 2 ) on an object plane on an object in the environment and two co-planar lines (P 1 -A, P 2 -B) on the object plane on the object, the robot comprising
a mobile base unit;
a camera mounted on the mobile base unit and configured to capture images of the environment, including images of the object;
a video decoder configured to receive the camera images from the camera, including images of the object and decode the images into decoded digital images, including decoded digital images of the object;
a processor; and
a memory storing instructions that, when executed by the processor, cause the robot to:
capture with the camera an image of the object in the environment;
convert the image of the object into a decoded digital image of the object disposed on an image frame;
identify on the decoded digital image of the object two observed points (op 1 , op 2 ) corresponding to the two co-planar points (P 1 , P 2 ) on the object;
identify on the decoded digital image of the object two observed image lines (op 1 - a , op 2 - b ) corresponding to the two co-planar lines (P 1 -A, P 2 -B) on the object;
project onto the decoded digital image of the object the two co-planar points (P 1 , P 2 ) on the object plane to obtain two projected co-planar image points (pp 1 , pp 2 ) on the decoded digital image; and project onto the decoded digital image the two co-planar lines (P 1 -A, P 2 -B) on the object plane to obtain two projected co-planar image lines (pp 1 - a , pp 2 - b ) on the decoded digital image;
determine a point projection error by comparing the two projected co-planar image points (pp 1 , pp 2 ) to the corresponding observed points (op 1 , op 2 ) on the decoded digital image of the object;
determine a line projection error by comparing the two projected co-planar image lines (pp 1 - a , pp 2 - b ) to corresponding observed image lines (op 1 - a , op 2 - b ) on the decoded digital image of the object; and
estimate a current robot pose if the point projection error and the line projection error are below a predetermined error threshold.
11. The robot of claim 10 wherein the memory further includes instructions that, when executed by the processor, cause the robot to combine the point projection error and the line projection error to establish a combined error, E total , and comparing the E total to the predetermined error threshold.
12. The robot of claim 11 wherein the memory further includes instructions that, when executed by the processor, cause the robot to performing a non-linear optimization on the two observed points (op 1 , op 2 ) relative to the two projected points (pp 1 , pp 2 ) and the two observed image lines (op 1 - a , op 2 - b ) and relative to the two projected co-planar image points (pp 1 , pp 2 ) before determining the combined error, E total .
13. The robot of claim 12 wherein if the combined error, E total , is not less than the error threshold, the memory further includes instructions that, when executed by the processor, cause the robot to further perform a non-linear optimization on the two observed points (op 1 , op 2 ) relative to the two projected points (pp 1 , pp 2 ) and the two observed image lines (op 1 - a , op 2 - b ) and re-determining the combined error, E total .
14. The robot of claim 11 wherein the point projection error is determined using the following formula:
E
points
=
∑
i
q
i
-
s
i
wherein q i are the projected points on image plane and s i are the observed points on the image plane.
15. The robot of claim 14 wherein the line projection error is determined using the following formula:
E
lines
=
n
ae
→
-
n
ae
′
→
+
n
bf
→
-
n
bf
′
→
wherein n ae and n′ ae are the projected lines on image plane and n bf and n′ bf are the observed lines on the image plane.
16. The robot of claim 15 wherein the combined point projection error and line projection error is determined using the following formula:
E
total
=
w
points
E
points
+
w
lines
E
lines
=
w
points
∑
i
q
i
-
s
i
+
w
lines
(
n
ae
→
-
n
ae
′
→
+
n
bf
→
-
n
bf
′
→
)
wherein W points and W lines are the weighting factors to balance the two kinds of errors.
17. The robot of claim 10 wherein the environment is a warehouse.
18. The robot of claim 17 wherein the object is an electric charging station for the robot in the warehouse.