Perception Modelling
3.5. Hierarchical Object-based SLAM
Recall that the key issues for successfully implementing SLAM in very large environ-ments are computational complexity, representation and data association. This work does not focus on the computational complexity issue since the recent work about this issue provides several promising algorithms. Instead, we intended to develop practical and re-liable algorithms for solving the representation and data association issues. In this section,
3.5 HIERARCHICAL OBJECT-BASED SLAM
−2 0 2 4 6
−2
−1 0 1 2 3 4 5
meter
meter
(a) The state estimates.
1 2 3 4 5
1.5 2 2.5 3 3.5 4 4.5 5
meter
meter
(b) The contour.
Figure 3.21. Registration results of the example in Figure 3.20 using the SCRIM al-gorithm. On the right: the states are indicated by ¤, and the final scan points are indicated by ×. On the left: the scan measurements over several frames are transformed to the same coordinate system using the registration results.
we will demonstrate that city-sized SLAM is feasible by using the hierarchical object based approach where SLAM is accomplished locally using grid-based approaches and globally using feature-based approaches. The data association issues will be addressed in Chapter 5.
Figure 3.22 shows an aerial photo of the CMU neighborhood where the data was col-lected. The line indicates the Navlab11 trajectory. Figure 3.23 shows the pose estimates from the onboard inertial measurement system. It illustrates that, even using high-end inertial measurement systems, the error of the robot pose estimate accumulates and map-ping using these estimates will diverge eventually. This data set contains about 36,500 scans. The visual images collected from the onboard three-camera system are only for vi-sualization.
Local Mapping using Grid-based approaches
Since feature extraction is difficult and problematic in outdoor environments, we ap-ply grid-based approaches for building the map. However, as addressed in the beginning of this chapter, the grid-based approaches need extra computation for loop-closing and all raw scans have to be used to generate a new global consistent map, which is not practical for online city-sized mapping. Therefore, the grid-map is only built locally.
After localizing the robot using the sampling and correlation based range image match-ing algorithm, the new measurement is integrated into the grid map. The Bayesian re-cursive formula for updating the grid map is computed by: (See (Elfes, 1988, 1990) for a
Figure 3.22. Aerial photo of the CMU neighborhood. The line indicates the trajec-tory of Navlab 11.
−1100 −1000 −900 −800 −700 −600 −500 −400
−100 0 100 200 300 400 500
meter
meter
Figure 3.23. Pose estimates from the inertial measurement system.
derivation.)
lxyk = log p(gxy| Zk−1, zk) 1 − p(gxy| Zk−1, zk)
= log p(gxy| zk)
1 − p(gxy| zk)+ lxyk−1+ l0xy (3.12)
3.5 HIERARCHICAL OBJECT-BASED SLAM
where g is the grid map, gxybe the occupancy value of a grid cell at hx, yi, l is the log-odds ratio, and
l0xy = log p(gxy)
1 − p(gxy) (3.13)
Practically, there are two requirements for selecting the size and resolution of grid maps: one is that a grid map should not contain loops, and the other is that the quality of the grid map should be maintained at a reasonable level. For solving the above example, the width and length of the grid map are set as 160 meters and 200 meters respectively, and the resolution of the grid map is set at 0.2 meter. When the robot arrives at the 40 meter boundary of the grid map, a new grid map is initialized. The global pose of the map and the corresponding distribution is computed according to the robot’s global pose and the distribution. Figure 3.24 shows the boundaries of the grid maps generated along the trajectory using the described parameters. Figure 3.25 shows the details of the grid maps, which contain information from both stationary objects and moving objects. The details of dealing with moving objects will be addressed in the following chapters.
−1200 −1000 −800 −600 −400
−300
−200
−100 0 100 200 300 400 500
meter
meter
1 2 3 4 5 6 7
8 9
10
11 12
13 14
Figure 3.24. Generated grid maps along the trajectory. The boxes indicate the boundaries of the grid maps.
Global Mapping using Feature-based Approaches
The first step to solve the loop-closing problem is to robustly detect loops or recognize the pre-visited areas. It is called the data association in the large problem in this dissertation or the revisiting problem (Stewart et al., 2003). Figure 3.26 shows that the robot entered the
(a) Grid-map 1 (b) Grid-map 2
(c) Grid-map 3 (d) Grid-map 4
Figure 3.25. Details of the grid maps. Gray denotes areas which are not occupied by both moving objects and stationary objects, whiter than gray denotes the areas which are likely to be occupied by moving objects, and darker than gray denotes the areas which are likely to be occupied by stationary objects.
explored area. Because of the accumulated pose error, the current grid map is not consis-tent with the pre-built map. In this section we assume that loops are correctly detected.
3.5 HIERARCHICAL OBJECT-BASED SLAM
The issues and solutions of the data association in the large problem will be addressed in Chapter 5.3.
−1200 −1000 −800 −600 −400
−300
−200
−100 0 100 200 300 400 500 600
meter
meter
16 15 17 18
Figure 3.26. The data association in the large problem.
For closing loops in real time, feature-based approaches are applied. Because the occu-pancy grid approach is used for local mapping, we have to develop a method to transform or decompose the occupancy grid map into stable regions (features) and a covariance ma-trix containing the correlation among the robot and the regions. Unfortunately, this is still an open question. Therefore, instead of decomposing the grid maps, we treat each grid map as a 3 degree-of-freedom feature directly. Figure 3.27 shows the result without closing and Figure 3.28 shows the result using the feature based EKF algorithm for loop-closing. Information from moving objects is filtered out in both figures. The covariance matrix for closing this loop only contains 14 three degree-of-freedom features.
Since we set the whole grid maps as features in the feature-based approaches for loop-closing, the uncertainty inside the grid maps is not updated with the constraints from de-tected loops. Although Figure 3.28 shows a satisfying result, the coherence of the overlay between grid maps is not guaranteed. Practically, the inconsistency between the grid-maps will not effect the robot’s ability to perform tasks. Local navigation can use the current built grid map which contains the most recent information about the surrounding environment.
Global path planning can be done with the global consistent map from feature-based ap-proaches in a topological sense. In addition, the quality of the global map can be improved
Figure 3.27. The result without loop-closing. Information from moving object is filtered out.
by using smaller grid maps to smooth out the inconsistency between grid maps. At the same time, the grid-maps should be big enough to have high object saliency scores in or-der to reliably solve the data association problem in the large.