globally consistent range scan alignment for environment mapping f. lu ∗ and e. milios department...
DESCRIPTION
Problem Definition Odometry information is not sufficient to determine relative pose scans. Incrementally integrating new frames of data to the global model, may result in an inconsistent model.TRANSCRIPT
![Page 1: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/1.jpg)
Globally Consistent Range Scan Alignment for Environment Mapping
F. LU AND E. MILIOS∗Department of Computer Science, York University, North York, Ontario, Canada
![Page 2: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/2.jpg)
Problem Definition
“In this paper, we address the issue of consistent alignment of data frames so that they can be integrated to form a world model.”
![Page 3: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/3.jpg)
Problem Definition Odometry information is not sufficient to determine relative pose
scans. Incrementally integrating new frames of data to the global model ,
may result in an inconsistent model.
![Page 4: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/4.jpg)
Related Work
HILARE project (Chatila and Laumond, 1985). Moutarlier and Chatila, 1989. Durrant-Whyte (1987, 1988a,1988b) Tang and Lee (1992)
![Page 5: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/5.jpg)
Approach to the Problem Statement A framework to continuously register range scans. Maintain all local frames of data as well as a network of spatial
relationships amongst them. Use pose relations as constraints, all pose as variables and solve the
final optimization problem.
![Page 6: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/6.jpg)
Deriving Pose Relations Pose relations can be directly obtained from odometry or aligning
pairwise scans of points.
The corresponding points from the two scans will form a constraint between the two poses.
The pose relation is derived from the overlapping potion of the two scans.
![Page 7: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/7.jpg)
Constructing and Combining Relations in a Network
A node of a network is a pose of the robot which can be defined as a 3 dimensional vector (x, y, θ)t
Links are determined by considering the amount of overlap between the range scans at two poses.
Considering the pose as a variable we build an objective function and solve by minimizing the function.
![Page 8: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/8.jpg)
Optimal Estimation Formulation
Mahalanobis Distance
X0,X1,....,Xn : n+1 nodes
Dij = Xi -Xj .
D = Dij + Δ Dij
Δ Dij= Random Gaussian Error
![Page 9: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/9.jpg)
Solution
We can express the eqn. As follows:D = HXW = (D-HX)t .C.-1 (D-HX)Then the solution for X which minimizes W is given by:X = (Ht C-1 H)-1.Ht .C-1D.
Cx =(Ht.C-1.H)-1 [Covariance of X ]
![Page 10: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/10.jpg)
Solution Continues..
Denoting HtC-1H by G, and expanding the matrix multiplicationswe can obtain d*d sub matrices of G as:
![Page 11: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/11.jpg)
Special Networks:A serial and a Parallel network
For the serial one , the derived estimate and covariance matrix are given by:
X2 = D01+D12
C2 = C01 + C12
For the parallel one:
X1 = (C'-1 +C''-1)-1(C'-1D' + C''-1D'')
C = (C'-1 + C''-1)-1
![Page 12: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/12.jpg)
Wheatstone Bridge Network
![Page 13: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/13.jpg)
Pose Compounding Operation
Assume that the robot starts at a pose Vb = (xb , yb , θb )t and it then
changes its pose by D = (x, y, θ)t relative to V b , ending up at a new pose Va = (xa , ya, θa )
t .
Then we say that pose Va is the compounding of Vb and D.
We denote it as:
Va = Vb D.⊕The coordinates of the poses are related by:
xa = xb + x cos θb − y sin θb
ya = yb + x sin θb + y cos θb
θa = θb + θ.
![Page 14: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/14.jpg)
Compounding Continues
It is also useful to define the inverse of compounding which takes two poses and gives the relative pose:
D = Va V⊕ b .
The coordinates are related by the following equations:
x = (xa − xb ) cos θb + (ya − yb ) sin θb
y = −(xa − xb ) sin θb + (ya − yb ) cos θb
θ = θa − θb .
We also want to define a compounding operation between a full 3D pose V b = (xb , yb , θb ) and a 2D position vector u = (x, y)t . The result is another 2D vector u' = (x' , y' )t . We still denote the operation as:
u = Vb u.⊕
![Page 15: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/15.jpg)
Pose Relations from Matched Scans Let Va and Vb be two nodes in the network .From the pairwise scan matching algorithm, we get a set
of pairs of corresponding points: uak , u
bk , k = 1, . . . , m,where the 2D non-oriented points ua
k , ub
k are from scan Sa and Sb , respectively.
ΔZ k = Va u⊕ ak − Vb u⊕ b
k = 0.
m
Fab(Va,Vb)= Σ || (Va u⊕ ak) - (Vb u⊕ b
k)||2
K= 1
![Page 16: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/16.jpg)
Pose Relations Continues In order to reduce Fab into
the Mahalanobis distance form, we linearize each term ΔZk . Let Va =( xa , ya , θa )
t , Vb = ( xb , yb, θb )t be
some close estimates of Va and Vb . Denote
ΔVa = Va − Va and ΔVb = Vb −Vb
Let uk = (xk , yk )t
= Va u⊕ ak ≈Vb u⊕ b
k
![Page 17: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/17.jpg)
Pose Relation Continues
![Page 18: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/18.jpg)
Pose Relation Continues
![Page 19: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/19.jpg)
Pose Relation Continues
![Page 20: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/20.jpg)
Pose Relations From Odometry
![Page 21: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/21.jpg)
Pose Relation Continues
![Page 22: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/22.jpg)
Pose Relation Continues
![Page 23: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/23.jpg)
Optimal Pose Estimation
![Page 24: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/24.jpg)
Optimal Pose Estimation
![Page 25: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/25.jpg)
Sequential Estimation
![Page 26: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/26.jpg)
Sequential Estimation
![Page 27: Globally Consistent Range Scan Alignment for Environment Mapping F. LU ∗ AND E. MILIOS Department of Computer Science, York University, North York, Ontario,](https://reader035.vdocuments.site/reader035/viewer/2022062523/5a4d1ae77f8b9ab059979508/html5/thumbnails/27.jpg)
Conclusion
In this paper, we formulated the problem of consistent range data registration as one of optimal pose estimation from a network of relations. Although we develop our method for mapping a 2D environment using 2D range scans, our formulation is general and it can be applied to the 3D case as well, by generalizing pose composition an linearization .