Method for mapping an environment

Patent metadata

Field	Value
Publication number	US-9412173-B2
Application number	US-201414783215-A
Country	US
Kind code	B2
Filing date	Apr 22, 2014
Priority date	Jun 21, 2013
Publication date	Aug 9, 2016
Grant date	Aug 9, 2016

Abstract

A method for mapping an environment comprises moving a sensor along a path from a start location (P 0 ) through the environment, the sensor generating a sequence of images, each image associated with a respective estimated sensor location and comprising a point cloud having a plurality of vertices, each vertex comprising an (x,y,z)-tuple and image information for the tuple. The sequence of estimated sensor locations is sampled to provide a pose graph (P) comprising a linked sequence of nodes, each corresponding to a respective estimated sensor location. For each node of the pose graph (P), a respective cloud slice (C) comprising at least of portion of the point cloud for the sampled sensor location is acquired. A drift between an actual sensor location (P i+1 ) and an estimated sensor location (P i ) on the path is determined. A corrected pose graph (P′) indicating a required transformation for each node of the pose graph (P) between the actual sensor location (P i+1 ) and the start location (P 0 ) to compensate for the determined drift is provided. The sequence of estimated sensor locations is sampled to provide a deformation graph (N) comprising a linked sequence of nodes, each corresponding to respective estimated sensor locations along the path. For at least a plurality of the vertices in the cloud slices, a closest set of K deformation graph nodes is identified and a respective blending function based on the respective distances of the identified graph nodes to a vertex is determined. Transformation coefficients for each node of the deformation graph are determined as a function of the 20 required transformation for each node of the pose graph (P) to compensate for the determined drift. Tuple coordinates for a vertex are transformed to compensate for sensor drift as a function of the blending function and the transformation coefficients for the K deformation graph nodes closest to the vertex.

First claim

The invention claimed is:

1. A method for mapping an environment comprising:
moving a sensor along a path from a start location (P 0 ) through said environment, said sensor generating a sequence of images, each image associated with a respective estimated sensor location and comprising a point cloud having a plurality of vertices, each vertex comprising an (x,y,z)-tuple and image information for said tuple,
sampling said sequence of estimated sensor locations to provide a pose graph (P) comprising a linked sequence of nodes, each corresponding to a respective estimated sensor location,
for each node of the pose graph (P), acquiring a respective cloud slice (C) comprising at least of portion of said point cloud for said sampled sensor location;
determining a drift between an actual sensor location (P i+1 ) and an estimated sensor location (P i ) on said path;
providing a corrected pose graph (P′) indicating a required transformation for each node of the pose graph (P) between said actual sensor location (P i+1 ) and said start location (P 0 ) to compensate for said determined drift;
sampling said sequence of estimated sensor locations to provide a deformation graph (N) comprising a linked sequence of nodes, each corresponding to respective estimated sensor locations along said path,
for at least a plurality of said vertices in said cloud slices, identifying a closest set of K deformation graph nodes and determining a respective blending function based on the respective distances of said identified graph nodes to a vertex;
determining transformation coefficients for each node of said deformation graph as a function of the required transformation for each node of the pose graph (P) to compensate for said determined drift; and
transforming tuple coordinates for a vertex to compensate for sensor drift as a function of said blending function and said transformation coefficients for said K deformation graph nodes closest to said vertex.

2. A method according to claim 1 wherein said step of determining a drift comprises:
for at least some images of said sequence of images, identifying at least one distinctive feature;
responsive to identifying matching distinctive features in two images, a first one of which was acquired at said start location and a second one of which was acquired at an estimated sensor location (P i ) along said path, aligning said matched distinctive feature in said images to determine the drift between an actual sensor location (P i+1 ) and said estimated sensor location (P i ).

3. A method according to claim 1 wherein said transformation coefficients include coefficients defining at least: a translation, a rotation and a projection for a vertex.

4. A method according to claim 3 wherein said transformation coefficients further include coefficients defining: stretching, shearing, contraction or twisting for a vertex.

5. A method according to claim 1 wherein said image information includes at least intensity information for a vertex.

6. A method according to claim 5 wherein said image information includes colour information for a vertex.

7. A method according to claim 1 wherein said transformed vertex comprises a vertex of the point cloud.

8. A method according to claim 1 wherein said transformed vertex comprises a vertex comprising real-world tuple coordinates and further comprising retrieving image information from said point cloud based on a transformation of said real-world tuple coordinates based on an inversion of said transformation coefficients.

9. A method according to claim 1 wherein determining said transformation coefficients comprises iteratively: calculating a set of candidate transformation coefficients for said deformation graph nodes (N); calculating an error value (E) as a function of the projected transformations of said pose graph nodes (φ( )) based on said set of candidate deformation graph transformation coefficients; and choosing a set of deformation graph transformation coefficients when said error is minimized.

10. A method according to claim 9 wherein said error value E comprises:
E=w rot E rot +w reg E reg +w con P E con P +w feat E feat

where w rot , w reg , w con P and w feat are blending coefficients and where
E rot is a function of the extent of rotation produced by a candidate set of deformation graph node coefficients;
E feat is a function of a difference between the location of a distinctive feature in an image associated with a pose graph node induced by a candidate set of deformation node coefficients and the required transformation of the distinctive feature;
E reg is a function of the variation in extent of translation and/or rotation produced by a candidate set of deformation graph node coefficients; and
E con P is a function of a difference between the locations of pose graph nodes induced by a candidate set of deformation node coefficients and the required transformation of the pose graph nodes.

11. A method according to claim 10 wherein:
w rot =1; w reg =10; w con P =100; w feat =100.

12. A method according to claim 10 wherein:

E

con
P

=

∑
i

⁢



ϕ
⁡

(

P

i
t

)

-

P

i
t

′



2
2

where, φ( ) is a translational mapping of a pose graph node (P i ) induced by a candidate set of deformation node coefficients and P′ i t is the required translation of the pose graph node (P i ).

13. A method according to claim 10 wherein:

E
feat

=

∑
i

⁢



ϕ
⁡

(

(

P

i
R

⁢

V
q

Assignees

• Nat Univ Of Ireland Maynooth
• Massachusetts Inst Technology

Inventors

• Leonard John Joseph
• Kaess Michael
• Mcdonald John
• Whelan Thomas John

Classifications

• G06T2207/20072

  Graph-based image processing · CPC title

• G06T2200/04

  involving 3D image data · CPC title

• G06T2200/08

  involving all processing steps from image acquisition to 3D model generation · CPC title

• G06T2207/10024

  Color image · CPC title

• G06T2207/10028

  Range image; Depth image; 3D point clouds · CPC title