Method for mapping an environment
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.
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
)
+
P
i
t
)
-
(
(
P
i
R
′
V
q
)
+
P
i
t
′
)
2
2
where φ((P i R V q )+P i t ) indicates a rotational (R) and translational (t) mapping of a distinctive feature V q from an image associated with pose graph node P i induced by a candidate set of deformation node coefficients and ((P′ i R V q )+P′ i t ) indicates the required mapping of the feature V q .
14. A method according to claim 10 wherein:
E
rot
=
∑
l
N
l
R
T
N
l
R
-
I
F
2
where N l is a set of deformation graph nodes, N l R is the rotation at N l for a candidate set of deformation node coefficients, and ∥ ∥ F 2 is the Frobenius norm.
15. A method according to claim 10 wherein:
E
reg
=
∑
l
∑
n
∈
(
N
l
)
N
l
R
(
N
n
g
-
N
l
g
)
+
N
l
g
+
N
l
t
-
(
N
n
g
+
N
n
t
)
2
2
where, N i g is the position of deformation graph node i, N i t is the translational component of the transformation and N l R is the rotational component at node N l for a candidate set of deformation node coefficients.