Mapping Topics: Topological Maps SLAM HMMs Revisited ● Additional reading: ● B. J. Kuipers & Y.-T. Byun. 1991. “A robot exploration and mapping strategy based on a semantic hierarchy of spatial representations.” Journal of Robotics and Autonomous Systems, 8: 47-63 ● J. J. Leonard, I. J. Cox, and H. F. Durrant-Whyte. “Dynamic map building for an autonomous mobile robot”. Int. J. Robotics Research, 11(4):286--298, August 1992. Issues ● Statement: – Given a set of observations of the world, what is the world like? ● Inputs: – Sequence of observations, or perhaps actions and observations, or perhaps actions, observations and vehicle motion/sensor models ● Outputs from different algorithms: – Graph of connected states – Graph of connected states, transition probabilities p(x i |x j ,a k ) and observation probabilities p(z i |x j ) – Description of landmark locations – Occupancy grid – List of points from range sensors ● Choices: – How to represent model “Closing the loop” Graphical Models States x 1 x 2 p(x j |a i , x i ) Z 2 b 1 Beliefs Z 1 Observations a 1 Actions p(z j |x i ) b 2 Hidden Observable ● Want to estimate p(x i |x j ,a k ), p(z i |x j ) instead of {x 1 ,x 2 ,...,x T } Topological Maps ● Idea: build map as graph of sensor signatures and control laws connecting nodes ● Actions are control laws ● Observations are...? ● p(?|?) = {0, 1} ● Layer geometric information on topology Extracted Topology Metric map layered on top Building the Topology ● Assume a set of distinctiveness measures, d 1 ,...,d n and a set of local control strategies 1. Choose a LCS 2. Follow LCS until maximum in one of d i is seen 3. Hill-climb on d i until maximum is reached 4. Move towards open space 5. Repeat A simple environment The Distinctiveness Surface Hill-Climbing Measures and Strategies ● Measures: – Range variance – Lateral range variance – Range symmetry – Temporal discontinuity – Number of directions of reasonable motion – Temporal change in number of directions of motion ● Strategies – Follow the midline – Move along Object on Left – Move along Object on Right – Blind step C A B D E P1 P2 WALL2 WALL3 WALL2 WALL1 WALL3 Pros and Cons ● Pro: ● Human-like maps ● Exploration strategy built into mapping process ● Purely on-line ● Con: ● Loop closure ● Will not necessarily map space completely ● Absence of decision-theoretic measures means no measure of map quality ● A large number of free parameters and heuristics ● Purely on-line ● Process model is now ● Measurement model is ● Linearising, we get ● where A, H, W and V are the Jacobians: The Extended Kalman Filter for Building Maps ),,( 111 ??? = tttt quxfx ),( ttt rxhz = ttttt ttttt ttt VrxxHxhz WqxxAxx uxfx +?+≈ +?+≈ = ??? ?? ) ~ ()0, ~ ( )?( ~ )0,,( ~ 111 11 ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = n nn n x f x f x f x f A L MOM L 1 1 1 1 ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = n mm n x h x h x h x h H L MOM L 1 1 1 1 ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = n nn n q f q f q f q f W L MOM L 1 1 1 1 ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = m mm m v h v h v h v h V L MOM L 1 1 1 1 Updates for the EKF ● Still using only first two moments. ● Process model: ● Measurement model: T ttt T tttt ttt WQWACAC uxfx += = ? ? ?? ? 1 11 )0,, ? ( ? ? ?? ? ?? ?= ?+= += tttt ttttt T ttt T ttt T ttt CHKIC xhzKxx VRVHCHHCK )( ))0, ? (( ?? )( 1 Robot Mapping Initial belief Action Measurement ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = y x y x 1 1 λ λ θξ ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?+?+ ?+?+ ?+?+ = x x q tqty tqtx quf 1 1 sinsin coscos ),,( λ λ θθθ θθ θθ ξ ? ? ? ? ? ? ? ? = θ t u ? ? ? ? ? ? = b r z ()() ? ? ? ? ? ? ? ? ? ? +? ? ? ? ? ? ? ? ? ? ? +?+? = ? ? ? ? ? ? = ? θ θ λ λ λλ ξ v x y vyx vh x y ryx 1 22 tan ),( bearing range 1?t ξ ? t ξ t ξ ? ? ? ? ? ? = y x λ λ λ Leonard, Cox and Durrant-White, 1992 The EKF for Robot Localization ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?? = 10000 01000 00100 00cos10 00sin01 θ θ t t A []00111=W ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = 2 1 2 1 2 1 2 1 1 1 1 1 1 0 r x r y r x r y r y r x r y r x H x y x y y x y x λ λ λ λ λ λ λ λ ()() ? ? ? ? ? ? ? ? ? ? +? ? ? ? ? ? ? ? ? ? ? +?+? = ? θ θ λ λ λλ ξ v x y vyx vh x y ryx 1 22 tan ),( ? ? ? ? ? ? = 10 01 V Prediction step Measurement step ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?+?+ ?+?+ ?+?+ = x x q tqty tqtx quf 1 1 sinsin coscos ),,( λ λ θθθ θθ θθ ξ Pros and Cons ● Pro: ● Very reliable ● Easy to implement if assumptions hold ● Gives very powerful framework for reasoning about quality of map ● Can be run on- or off-line ● Landmark representation somewhat intuitive to humans ● Con: ● Very sensitive to data association errors ● Point features ● Linear-Gaussian world model ● Quadratic complexity HMMs Revisited 3) How do we adjust the model parameters λ=(A,B,π) to maximize P(O|λ)? ● Assume a topology of states X ● Want to learn p(x i |x j , a k ), and p(z i |x j ) from a sequence O(z 1 ,a 1 , z 2 ,a 2 ,…, z T ,a T ) ● Approximate idea: ● Assume we have O(z 1 ,a 1 ,x 1 ,z 2 ,a 2 ,x 2 ,…, z T ,a T ,x T ) ● Use counting to estimate p(x i |x j , a k ), and p(z i |x j ) ● Re-estimate x 1 ,x 2 ,…,x T ● Repeat ● How to do this efficiently and correctly? Remember HMM Basic Problem 1 )|()( 11 ii xzpi πα = )|(),|()()( 1 || 1 1 it X j tijtt xzpaxxpji + = + ? ? ? ? ? ? = ∑ αα ∑ = = || 1 )()|( X i t iOp αλ 1)( =i T β )()|(),|()( 11 || 1 jxzpaxxpi tjt X j tijt ++ = ∑ = ββ Backwards terms: HMMs Revisited 1. Assume a prior model 2. Compute probability of transition from x i to x j at time t and probability of all transitions at time t ∑∑ == + + = N i N j tjtjit tjtjit jxzpxxpi jxzpxxpi 11 1 1 )()|()|()( )()|()|()( βα βα ∑ = = i j tt jii 1 ),()( ζγ We’ve left out actions here. Actions imply an extra index on ζ and some extra accounting for time indices. HMMs Revisited 3. Update parameters using expected counts 4. Repeat until model stops changing ∑ ∑ ? = ? = = 1 1 1 1 )( ),( )|( T t t T t t ji i ji xxp γ ζ ∑ ∑ = = == = T t t T t itt ji j zzj xzp 1 1 )( )()( )|( γ δγ s i a ij t-1 t t t+2 b j s j (o t+1 ) (i)a b t+1 t+1 ( j) s i a ij t-1 t t t+2 b j s j (o t+1 ) (i)a b t+1 t+1 ( j) + = tjtjit t Op jxzpxxpi ji 1 )|( )()|()|()( ),( λ βα ζ Pros and Cons ● Pro: ● Most general ● Gives very powerful framework for reasoning about many, many different kinds of problems ● Con: ● Computationally complex (iterative procedure for a single update) ● Can be data-inefficient ● Subject to local minima ● Purely off-line ● Hard to extract human-oriented map