Making HRI accessible to everyone through making hri accesible to everyone through online videos
HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014...
Transcript of HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014...
![Page 1: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/1.jpg)
Vehicle Localization based on Lane Marking Detection
Yuncong ChenUCSD
HRI intern 2014
![Page 2: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/2.jpg)
Overview
● Odometry (noisy GPS / IMU for now)
● Monocular camera
● Lane level map
● lateral localization on highway● give correct estimate on merge
/ split points
GoalInput
Assumptions
● road surface is flat
![Page 3: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/3.jpg)
Coordinate System
North / East / Down Longitude / Latitude
local plane origin = first gps position
GPS AlgorithmCalifornia State Plane
Map
![Page 4: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/4.jpg)
Map no semantic information, interpolate
![Page 5: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/5.jpg)
Particle Filter
● Motion model
current pose
previous pose
GPS / odometry
● Observation model
mapcurrent image
pose = (north, east, yaw)
map = a set of points labeled by marking groups
![Page 6: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/6.jpg)
Particle Filter
propagate using motion model
weight each particle by its likelihood computed from observation model
resample particles according to their weights
all with same weight here
![Page 7: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/7.jpg)
Motion Model
initialrotationnoise
translation noise
finalrotationnoise
● rotations and translation computed from odometry
![Page 8: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/8.jpg)
Observation Modelproject map points to bird’s-eye view
Given the vehicle pose, our bird’- eye view image is expected to look like this ...
![Page 9: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/9.jpg)
Observation Modelproject map points to bird’s-eye view
Given the vehicle pose, our bird’- eye view image is expected to look like this ...
![Page 10: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/10.jpg)
Observation Model
inverse perspective transform filter Hough line fitting
… while what we really observe is ...
![Page 11: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/11.jpg)
Observation Model
expected
observed
match lines
![Page 12: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/12.jpg)
Maximum Bipartite Matching
observed
expected
![Page 13: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/13.jpg)
… not so simpleorder must be consistent matches cannot be
too far away some map lines may not be detected in the image
# candidate matchings
![Page 14: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/14.jpg)
Likelihood Scoremap line i
detected linematched to map line i
distance between the line pair
prob. of not detecting a map line
total number of map lines
![Page 15: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/15.jpg)
Speed Up Matching share among particles
(0,0)(1,1)(2,2)(3,3)(4,4)
(0,1)(2,3)(3,4)
(0,0)(1,1)(2,2)(3,3)(4,4)
(0,0)(2,1)(3,2)(4,3)
(0,0)(1,1)(2,2)(3,3)(4,4)
● Sample to obtain a very small set of candidate matchings
● For the rest of the particles, only evaluate these candidate matchings
● Exploit spatial correlation of matchings among nearby particles
(map, detected)(0,0)(1,1)(2,2)(3,3)(4,4)(0,0)(2,1)(3,2)(4,3)(0,1)(2,3)(3,4)
● Preferable to sample particles spread out in space.
![Page 16: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/16.jpg)
Speed Up Matching search in previous map lines’ extent
map group 16
map: ipmg16: 0g3: 1g7: 2, 3g8: 2, 3g11: 3, 4
● Keep track of extent of every map line
● For a new set of detected lines, search matchings for each map line only within its extent
● Exploit temporal invariance of matchings for a single particle at different times
map group 3map group 7map group 8map group 11
![Page 17: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/17.jpg)
Process Images
inverse perspective transform filter Hough line fitting
![Page 18: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/18.jpg)
Inverse Perspective Mapping
pitch
yaw
height
measured by hand
![Page 19: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/19.jpg)
Inverse Perspective Mapping
![Page 20: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/20.jpg)
Inverse Perspective Mapping
![Page 21: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/21.jpg)
Inverse Perspective Mapping
![Page 22: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/22.jpg)
Top-hat Filter
high response if one side of an edge is very dark
![Page 23: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/23.jpg)
Top-hat Filter
*
*
threshold
threshold
&
high response if one side of an edge is very dark
more robust for detecting dark-bright-dark patterns
![Page 24: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/24.jpg)
Steerable Filter Second derivative of Gaussian
separablerotated to arbitrary angle
![Page 25: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/25.jpg)
Map-guided FilteringSC
logic OR
...
![Page 26: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/26.jpg)
Steerable vs. Top-hat noisy image
top-hat map-guided steerable
![Page 27: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/27.jpg)
Take Advantage of Map
Motion model● more likely to go along the current lane● cannot move beyond road edges
Observation model● map-guided image filtering● map-guided line fitting
![Page 28: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/28.jpg)
Hough Transform Line Fitting
25 line segments detected by OpenCV’s probabilistic Hough transform
6 lines remains after merging
![Page 29: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/29.jpg)
Experiments on straight lanes
stra
ight
avg lateral error: 0.22, max: 1.35
![Page 30: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/30.jpg)
Straight lanes
![Page 31: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/31.jpg)
Deal with Curved Lanes
curved
avg lateral error: 0.25, max: 0.98
![Page 32: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/32.jpg)
Deal with Curved Lanes● detect whether
the line is a curve (i.e. residual of a linear regression is large)
● if so, match only the bottom segment
![Page 33: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/33.jpg)
Steerable vs. Top-hat straight
steerable, avg lateral error 0.23, max 1.15 top-hat, avg lateral error 0.2, max .86
![Page 34: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/34.jpg)
Steerable vs. Top-hat curve
steerable, avg lateral error 0.3, max 0.79avg lon. error 0.7, max 1.55
top-hat, avg lateral error 0.47, max 1.8avg lon. error 0.67 max 2.83
![Page 35: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/35.jpg)
Effect of the Number of Particles
![Page 36: HRI intern 2014 UCSD Vehicle Localization based on Lane ...cseweb.ucsd.edu/~yuc007/slam/HRI 2014 Presentation.pdf · HRI intern 2014. Overview Odometry (noisy GPS / IMU for now) Monocular](https://reader034.fdocuments.in/reader034/viewer/2022042301/5ecbe7cf7abb7525a74eaa22/html5/thumbnails/36.jpg)
Issues and Extensions
● shadows● more general markings (urban environment)
○ stop-lines (longitudinal correction)○ curved lanes○ model-free
● investigate how number of particle affects performance