Dealing with Laser Scanner Failure: Mirrors and Windows
Shao-Wen Yang and Chieh-Chih Wang
Abstract— This paper addresses the problem of laser scanner failure on mirrors and windows. Mirrors and glasses are quite common objects that appear in our daily lives. However, while laser scanners play an important role nowadays in the field of robotics, there are very few literatures that address the related issues such as mirror reflection and glass transparency.
We introduce a sensor fusion technique to detect the potential obstacles not seen by laser scanners. A laser-based mirror tracker is also proposed to figure out the mirror locations in the environment. The mirror tracking method is seamlessly integrated with the occupancy grid map representation and the mobile robot localization framework. The proposed approaches have been demonstrated using data from sonar sensors and a laser scanner equipped on the NTU-PAL5 robot. Mirrors and windows, as potential obstacles, are successfully detected and tracked.
I. INTRODUCTION
Simultaneous localization and mapping (SLAM) is the process by which a mobile robot can build a map of the environment and, at the same time, use this map to compute its location. As the SLAM problem has attracted immense attention in the mobile robotics literature, there has been a large variety of sensors available for performing SLAM, such as sonar, laser scanner, infrared sensor, monocular vision and stereo vision. The past decade has seen rapid progress in solving the SLAM problem, and the laser scanners are at the core of most state-of-the-art successful robot systems, such as Sandstorm and H1ghlander [1], the autonomous vehicles in the DARPA Grand Challenge. Due to high speed of light and narrow beam angle, the laser scanners are appropriate for high precision applications in the field of robotics.
The laser scanner finds the distance of a surface by timing the round-trip time of a pulse of light. Laser scanners can send light photons toward an object and only receive a small percentage of those photons back via the optics that they use.
The reflectivity of an object is based upon the objects color or terrestrial albedo. A white surface will reflect lots of light and a black surface will reflect only a small amount of light.
Transparent objects such as glasses will only refract the light and give the range information of an object behind the transparent object. Mirrors, particularly, are flat surfaces and reflect off almost all the light so that the angles of incidence and reflection are equal. Most notably, in geometry, the mirror image of an object is the virtual image formed
Shao-Wen Yang is with the Department of Computer Science and Information Engineering, National Taiwan University, Taipei, Taiwan [email protected]
Chieh-Chih Wang is with Faculty of the Department of Computer Science and Information Engineering, and the Graduate Institute of Net- working and Multimedia, National Taiwan University, Taipei, Taiwan [email protected]
Fig. 1. NTU-PAL5 robot
by reflection in a plane mirror. The mirror image will be formed that seems to be behind the mirror is of the same size of the object from where the rays come from. Figure 2 shows laser range scans that illustrate the circumstances of glasses and mirror reflection. Laser scanners can miss obstacles which are with light-reflected or light-absorbed materials. As addressed in [2], the detection of mirrors and glass objects can be problematic.
Service robots will be required to run autonomously in a variety of real environments, as illustrated in Figure 2(b), 2(d) and 2(f). As laser scanners have become the major per- ceptual sensors, nowadays, equipped by the mobile robots, objects such as mirrors and windows can pose a real danger to robots with limited sensing capability, such as a robot equipped with only laser scanners.
To make robots fully autonomous in environments with mirrors and windows, detection and modeling of these ob- jects are critical. J¨org [3] proposed to use laser scanner measurements to filter out spurious sonar measurements. The spatial information is counted and stored in an accumulated grid map. Dudek et al. [4] introduced an approach to extract line segments in laser scans and sonar readings.
In this paper, problems of both mirrors and windows are addressed and integrated into the localization, mapping, and navigation framework. We introduce a sensor fusion technique to detect potential obstacles using sonar sensors and a laser scanner. The spatio-temporal occupancy infor- mation is accumulated and used to provide reliable scene understanding. A laser-based approach is proposed to detect and track mirrors in the environment where the geometric property of laser light is employed. With the fusion of the laser scanner and the sonar sensors, the mirrors and windows can be identified and properly modeled. The solution to the robot navigation problem in environments with mirrors and windows is also proposed. A unified framework is
and sixteen SensComp 600 Series Environmental Transducer sonar sensors.
II. SENSORFUSION
As the light can be reflected off the mirrors and penetrate the windows, mobile robots equipped with only laser scan- ners might not be capable to deal with the real environments.
The sonar, oppositely, is capable of detecting those objects that the laser scanner fails. The main drawbacks in sonar sensing are specularity, wide beam width, and frequent misreadings due to either external ultrasound sources or crosstalk [3]. The sonar aids the laser scanner by detecting mirrors and windows not seen by the laser scanner, while the laser scanner aids the sonar by retrieving more detailed environment information.
In this paper, we propose to perform sensor fusion by maintaining two individual occupancy grid maps [5]. Instead of making hard decisions at every time step, the occupancy grid maps are utilized to accumulate the temporal infor- mation of the sensor readings. The grid maps shown in Figure 3 are built using the data collected in the environment with mirrors and windows. It is observed that mirrors and windows are objects which are probable to be seen by the sonar sensor, but less likely by the laser scanner. Let Ml and Ms be the occupancy grid maps built using data from a laser scanner and sonar sensors, respectively, as illustrated in Figure 3(a) and 3(b). Each grid cell (x, y) is determined as a potential obstacle if the following inequalities hold,
Mlx,y< κl (1)
Msx,y> κs (2)
where κl and κs are pre-defined probabilities. The values of κl and κs can be obtained according to a priori probabilities used in the occupancy grid map representation. In our experiments, κl is 0.05 and κs is 0.95. At every time step, the sensor fusion map is calculated accordingly. The probability Mx,y of the grid cell (x, y) in the sensor fusion map M is Msx,y if Equation 1 and 2 hold; otherwise, Mlx,y.
Figure 3(c) illustrates the sensor fusion map. It is clear that most of the mirror and window locations are successfully identified in comparison with the laser scanner map. With the use of the sensor fusion map, the navigation of the mobile robot can be collision-free.
However, in the sensor fusion map, mirrors and windows make no difference. As illustrated in Figure 3(c), it is less likely for the robot to distinguish whether the area behind a potential obstacle is real objects or a mirror image. In the next section, we propose an approach to accumulate the positive information from the laser scanner and eliminate the fake counterpart.
distance-based criterion is used to determine the gaps in a laser scan. Once a couple of gap locations are determined, the geometric property of mirror symmetry is used to restore the spatial information of the reflected scan points. The likelihood field sensor model [6] is applied to calculate the possibility that a gap is indeed a mirror. The iterative closest points (ICP) algorithm [7] is then utilized in representing the mirror prediction. For those verified potential mirror locations, an ICP-based matching technique [8] is then applied to measure the uncertainties of the locations that the mirrors might appear.
A. Prediction
The mirror prediction method employs the fact that the mirrors are usually framed. In other word, the boundaries of a mirror are visible to the laser scanner. Firstly, we define that the gaps are discontinuities of range measurements within a laser range scan. Let z be the measurement of the laser range finder. A gap Gi, j is consisted of two measurement {zi, zj|i < j} such that
zi+1− zi> d (3)
zj−1− zj> d (4)
|zk− zk+1| ≤ d for i< k < j − 1 (5) where zi is the i-th range measurement of the laser range finder and d is a pre-determined constant. In our experiments, d is 1.5 meter. The line with endpoints {pi, pj} is thus considered to be a potential mirror where pi and pj are Cartesian coordinates of range measurements zi and zj, respectively, in the robot frame.
However, the proposed method can miss a mirror without any frame. In this case, the mirror is invisible to laser scanners. The direct solution might just explicitly enumerate all of the possible mirrors within the environment. Since enumeration in the continuous space is computationally intractable, the na¨ıve implementation is to discretize the continuous space. However, most of the enumerations are obviously impractical and, clearly, the method is still com- putationally intractable.
B. Verification
For each of the gap Gi, jwith endpoints {pi, pj}, the set of measurements {zi+1, zi+2, . . . , zj−1} is restored in accordance with the geometric property of mirror symmetry.
Let ei, jbe the line with endpoints piand pj, e0,kbe the line with endpoints pkand the origin, and pi, j,kbe the intersection point between the two lines ei, jand e0,k. The reflected point
˜
pkwith respect to the k-th range measurement zkis calculated such that
ρ (0, pk) = ρ 0, pi, j,k + ρ pi, j,k, ˜pk
(6)
∠ 0, pi, j,k, pi = ∠ pj, pi, j,k, ˜pk
(7)
0 1 2 3 4 5 6 7 8 9 10
−5 0
5
x−axis (m)
y−axis (m)
(a) Laser range scan
(b) Visual image of the scene
0 1 2 3 4 5 6 7 8 9 10
−5 0
5
x−axis (m)
y−axis (m)
(c) Laser range scan
(d) Visual image of the scene
0 2 4 6 8 10 12 14
−8
−6
−4
−2 0 2 4 6
x−axis (m)
y−axis (m)
(e) Laser range scan
(f) Visual image of the scene
Fig. 2. Environments with mirrors and windows. (a), (c) and (e) show laser range scans in the environments with mirrors or glasses which are marked with rectangles. (b), (d) and (f) are the camera images of (a), (c) and (e), respectively, for visual reference. In (a), there is a mirror placed on the right side of the corridor. In (c), there is a long glass on the right side of the corridor. In (e), there is a mirror pillar on the right side of the room.
−5 0 5 10 15 20
−20
−15
−10
−5 0 5
x−axis (m)
y−axis (m)
(a) Laser Scanner Map
−5 0 5 10 15 20
−20
−15
−10
−5 0 5
x−axis (m)
y−axis (m)
(b) Sonar Map
−5 0 5 10 15 20
−20
−15
−10
−5 0 5
x−axis (m)
y−axis (m)
(c) Sensor Fusion Map
Fig. 3. Occupancy grid maps. (a) and (b) show the map obtained by using the data from the laser scanner and the sonar sensors, respectively. (c) depicts the map obtained by fusion of the laser scanner and the sonar sensors. Rectangles show the potential obstacles not seen by using only the laser scanner.
The potential obstacles are successfully identified with the use of the sensor fusion map. However, the robot still can not distinguish the differences in the sensor fusion map between mirrors and windows.
where ρ(·, ·) is the Euclidean distance function,∠(p1, p2, p3) is the angle function calculating the angle between vectors
−−→p1p2 and−−→p3p2 and 0 is the origin.
The likelihood `i, j of the reflected scan points { ˜pi+1, ˜pi+2, . . . , ˜pj−1} is then calculated. A gap Gi, j with likelihood `i, j greater than or equal to ` is considered probable to be a mirror Mi, j where ` is a pre-defined constant probability. In our experiments, ` is 0.5, meaning that a gap with at least 50% confidence is considered as a possible mirror location. The mean and the covariance
matrix of the predicted mirror is then calculated.
C. Representation
There are several ways to represent the mirror prediction.
Intuitively, the mirror is a line segment and can be described with the two endpoints of the mirror. The filtering algorithm can update the endpoints with the associated mirror mea- surement separately. However, according to the basic light properties, whether the laser beam reflects back or reflects off is highly relevant to the smoothness of the mirror surface
of the mirror. The instability of the measurements around mirrors are illustrated in Figure 4. Therefore, instead of storing the endpoints of the mirror measurement directly in the state vector, we propose to represent the mirror with the line model and store the corresponding endpoints separately.
We propose to represent mirrors as line segments. In the state vector, line segments are represented by an angle and the distance of the closest point on the line to the origin of the robot frame. The endpoints of a line segment are not placed within the state vector but stored separately. The mean of the line segment of Mi, j with respect to the robot frame is given as
µMRi, j= αMRi, j λMRi, j
!
=
arctan ¯yi, j,k
¯ xi, j,k
qx¯2i, j,k+ ¯y2i, j,k
(8)
where ¯xi, j,k and ¯yi, j,k are x-y coordinate of the closest point on the line to the origin.
To measure the uncertainty of a mirror prediction, we propose to utilize the image registration technique. With the use of the ICP-based matching technique, the covariance matrix of the mirror measurement can be given as
ΣRMi, j =
σα2+ ∆θ2 0 0 σρ2+ ∆x2+ ∆y2
(9) where σα and σρ are pre-determined values of the measure- ment noise for the covariance matrix, and ∆x, ∆y, and ∆θ is the registration result using the ICP algorithm in which { ˜pi+1, ˜pi+2, . . . , ˜pj−1} and the whole range scan are aligned.
The values of σα and σρ can be obtained by taking into account the modeled uncertainty sources. In our experiments, σα is 3 degree and σρ is 0.2 meter.
Figure 5 illustrates the detection results that gaps in the range scans are identified. With the restoration of the reflected points, the mirrors can be discriminated.
IV. MIRRORTRACKING
In this section, we describe the method to update the mirror locations. The mirror predictions at different time steps are integrated using the extended Kalman filter (EKF).
As the endpoints are not stored in the state vector, the update stage is separated into two stages: the line update stage and the endpoints update stage.
A. Line Update
The mean and the covariance matrix of the line model is firstly transformed into the global coordinate, which is given as
µMi, j =
αMR
i, j+ θt
xtcos αMR
i, j+ θt
+ ytsin αMR
i, j+ θt
! (10) ΣMi, j= JxtPtJxTt+ JMi, jΣRMi, jJMTi, j (11)
using a validation gate defined by the Mahalanobis distance.
The standard EKF process is then applied to update the mean and the covariance matrix of the mirror estimate.
B. Endpoints Update
After the line model of a mirror estimate is updated, the endpoints of the mirror should be updated as well. Let Mi, jt and Mt+1u,v be the associated mirror measurements at time t and t + 1, {pti, ptj} and {pt+1u , pt+1v } be the endpoints of Mi, jt and Mu,vt+1, respectively, ˆMt+1 be the updated mirror estimate at time t + 1, and ˆet+1 be the corresponding line model of the updated mirror estimate. We can compute the point set ˆP = { ˆpti, ˆptj, ˆpt+1u , ˆpt+1v } which are the closest points from P = {pti, ptj, pt+1u , pt+1v } to the line ˆet+1.
As described in section III-C and illustrated in Figure 4, according to the basic light properties, reflection of the laser beam at the mirror location is unstable. The observed endpoints of a mirror are usually not the true counterparts and thus the distance between the endpoints of the mir- ror prediction is never longer than the true distance. We take advantage of aforementioned a priori knowledge to accommodate this phenomenon. The endpoints of the mirror estimate ˆMt+1can be obtained by finding a pair of points in ˆP such that the distance between these two points is maximum, which can be expressed as
ˆpt+11 , ˆpt+12 = argmaxp
1,p2∈Pρ (p1, p2) (12)
where ˆpt+11 and ˆpt+12 are the resulting endpoints of the mirror estimate ˆMt+1.
Figure 6 illustrates the mirror tracking result that a mirror is correctly detected and tracked.
V. MAPPING, LOCALIZATION,ANDNAVIGATION
In this section, we describe the mapping, localization and navigation problems in environments with mirrors. Without the mirror tracking process, as the mirror images are treated as parts of real environments, the map building and the robot localization tasks can still be performed well. However, the inconsistency between the real environment and the map containing mirror images makes the robot navigation problematic. The robots should be able to figure out mirror images and avoid colliding with mirrors. To deal with the phenomenon of mirror reflection, the mirror images within a grid map should be detected and corrected accordingly. As the occupancy grid map represents the configuration space (C -space) of the robot, without the correction process, the inconsistency between the map and the environment can be problematic for robot navigation. Therefore, grid cells at the mirror locations should be updated as occupied such that the reliability and the safety of the robot can be ensured.
By introducing the mirror tracking technique, mirrors in the environment are successfully detected and tracked and, at the
0 1 2 3 4 5 6 7 8 9 10
−5 0
5
x−axis (m)
y−axis (m)
(a) Scan 17
0 1 2 3 4 5 6 7 8 9 10
−5 0
5
x−axis (m)
y−axis (m)
(b) Scan 20
0 1 2 3 4 5 6 7 8 9 10
−5 0
5
x−axis (m)
y−axis (m)
(c) Scan 44
Fig. 4. In this figure, the laser scans at different time steps are shown. The scene is the same as that shown in Figure 2(b) where a mirror is placed on the right side of the corridor. The rectangles indicate the true mirror locations. In (a), almost all of the emitting laser beams reflect back directly from the mirror. In (b), the laser beams are missing due to the long travel distance of the light. In (c), however, the laser beams reflect off from the mirror surface.
The measurements around the mirror location are not stable due to the angle of incidence and the smoothness of the mirror surface.
0 1 2 3 4 5 6 7 8 9 10
−5 0
5
x−axis (m)
y−axis (m)
(a) Mirror detection result
0 2 4 6 8 10
−5 0
5
x−axis (m)
y−axis (m)
(b) Mirror detection result
0 5 10 15
−6
−4
−2 0 2 4 6
x−axis (m)
y−axis (m)
(c) Mirror detection result
Fig. 5. Mirrors are detected using the property of mirror symmetry. The robot is at the origin of the coordinate and heads towards the positive x-axis.
Dots are the raw range measurements, where the heavy dots (in red) are measurements not identified as a mirror image, and the light dots (in cyan) are measurements with false range information due to mirror reflection. Lines indicate the predicted line models of the mirrors where the thick lines (in black) are the verified mirror locations and the thin lines (in magenta) are those less likely to be mirror locations. Circles and crosses are the restored reflected points with respect to the verified mirrors and predicted mirrors, respectively.
same time, the mirror estimates are used to build the map consistent with the real environment.
A. Mapping
In the online phase, the mirrors are detected and tracked while the SLAM process is performed. The map can be further refined with the offline SLAM technique such as [9]. Accompanying the post-processing process, the range scan data can be revised such that the mirror images are eliminated. In the offline phase, each of the measurements with laser beam penetrates through a mirror estimate is updated as the distance to the mirror surface.
Figure 7 and 8 illustrates the process of the offline phase.
In Figure 7(a) and 8(a), the maps of the online phase are represented. In Figure 7(b) and 8(b), the resulting maps after the offline phase is performed are shown where the grid cells located behind a mirror are corrected. The false estimates are also successfully eliminated. The less certain
mirror estimates are discarded in the offline phase and the post-processing process only take into account the confident mirror estimates.
B. Localization
For mobile robot localization such as Markov localiza- tion and Monte Carlo localization, in comparison with the above post-processing process, the pre-processing process is required to take the mirror information into account. The pre-processing process eliminate the mirror image within a laser scan by applying the mirror symmetry, as described in section III-B. The updated laser scanner measurements are then used to perform the localization task.
VI. CONCLUSION ANDFUTUREWORK
A. Conclusion
In this paper, we address the problem of mirrors and win- dows that usually exist in our daily lives. The sensor fusion
−5 0 5 10 15 20
−20
−15
−10
−5
x−axis (m)
y−axis (m)
(a) Map before post-processing
−5 0 5 10 15 20
−20
−15
−10
−5
x−axis (m)
y−axis (m)
(b) Map after post-processing
Fig. 7. Map is refined with the use of the mirror tracking result. The scene is the same as that shown in figure 2(b). The false mirror estimates are eliminated according to the divergence of the false estimates. (a) shows the occupancy grid map before the offline phase, where (b) depicts the occupancy grid map after the offline phase.
0 5 10 15
−10
−8
−6
−4
−2 0 2 4 6 8
x−axis (m)
y−axis (m)
(a) Map before post-processing
0 5 10 15
−10
−8
−6
−4
−2 0 2 4 6 8
x−axis (m)
y−axis (m)
(b) Map after post-processing
Fig. 8. Map is refined with the use of the mirror tracking result. The sensor data is collected at the demo room of Taiwan Shin Kong Security, as depicted in Figure 2(f). (a) shows the occupancy grid map before the offline phase, where (b) depicts the occupancy grid map after the offline phase.
0 1 2 3 4
−4.5
−4
−3.5
−3
−2.5
−2
−1.5
−1
−0.5 0
x−axis (m)
y−axis (m)
Fig. 6. A mirror is tracked with EKF using line feature. The occupancy grid map of the environment is shown, where the rectangle (filled with blue) indicates the robot pose, the lines (in red) are the line models of the mirrors, the ellipses (in green) show the 2σ covariances of the line models, and the thick lines (in red) indicate the mirror locations.
SLAM is presented that utilizes the information from sonar sensors and a laser scanner concurrently. The occupancy grid map representation is used to explore the potential obstacles not seen by the laser scanner. The property of sonar sensors is exploited to aid the detection of mirrors and windows. Only with the use of sensor fusion SLAM, can the safety and the reliability of the mobile robot be ensure. The successful fusion is also demonstrated in section II.
Acting autonomously for the mobile robot in a wide variety of environments is difficult, especially in environ- ments with transparent objects, light-reflected objects or light-absorbed objects. To the authors’ best knowledge, the solution to the problem of mirrors and windows has not yet been addressed before. Based on the state-of-the-art SLAM technique, the primary contribution of this paper is to introduce the mirror tracking framework. The mirror detection method utilizes the property of mirror symmetry to evaluate the possibility of a mirror prediction. The mirror representation models the location of a mirror prediction with the line model and stores the endpoints separately.
Localization, mapping and navigation with mirror tracking is also presented in section V. The ample experimental results using data from sonar sensors and a laser scanner collected by the NTU-PAL5 robot have demonstrated the feasibility and effectiveness of the proposed approaches.
B. Future Work
Future work will include an approach to guess the pos- sible mirror locations based on the proposed sensor fusion approach. Due to less precision of the sonar sensor, extracting and reconstructing of disjointed line segments are required to generate the mirror prediction. Also, it would be of interest to study the special cases: multiple reflections of mirrors, curved mirrors, and scenes with symmetric structure.
Multiple reflections and non-flat mirrors are difficult to understand and visualize. Aside from the geometric prob- lems, scenes with symmetric structure can be a problem as well. The mirror behaves just as a glass placed in the proper location within an environment with symmetric structure.
It is indistinguishable if nothing within the eyesight of the sensors, or even our eyes, is different. It is tricky and can be discriminated when the robot itself enters the area within its eyesight.
VII. ACKNOWLEDGMENTS
We acknowledge the helpful suggestions by anonymous reviewers. This work was partially supported by grants from Taiwan NSC (#96-2628-E-002-251-MY3, #96-2218-E-002- 008, #96-2218-E-002-035); Excellent Research Projects of National Taiwan University (#95R0062-AE00-05); Taiwan DOIT TDPA Program (#95-EC-17-A-04-S1-054); Taiwan ITRI and Intel.
REFERENCES
[1] C. Urmson, J. Anhalt, D. Bartz, M. Clark, T. Galatali, A. Gutierrez, S. Harbaugh, J. Johnston, H. Kato, P. L. Koon, W. Messner, N. Miller, A. Mosher, K. Peterson, C. Ragusa, D. Ray, B. K. Smith, J. M.
Snider, S. Spiker, J. C. Struble, J. Ziglar, and W. R. L. Whittaker,
“A robust approach to high-speed navigation for unrehearsed desert terrain,” Journal of Field Robotics, vol. 23, no. 8, pp. 467–508, August 2006.
[2] A. Dioso and L. Kleeman, “Advanced sonar and laser range finder fusion for simultaneous localization and mapping,” in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Sendai, Japan, September 2004.
[3] K.-W. J¨org, “World modeling for an autonomous mobile robot using heterogenous sensor information,” Robotics and Autonomous Systems, vol. 14, no. 22, pp. 159–170, 1995.
[4] G. Dudek and C. Zhang, “Vision-based robot localization without explicit object models,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). Minneapolis, MN:
IEEE Press, 1996.
[5] A. Elfes, “Occupancy grids: a probabilistic framework for robot per- ception and navigation,” Ph.D. dissertation, Electrical and Computer Engineering/Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, 1989.
[6] S. Thrun, “A probabilistic online mapping algorithm for teams of mobile robots,” International Journal of Robotics Research, vol. 20, no. 5, pp.
335–363, April 2001.
[7] P. J. Besl and N. D. McKay, “A method for registration of 3-D shapes,” IEEE Transaction on Pattern Analysis and Machine Intelli- gence, vol. 12, no. 2, pp. 239–256, 1992.
[8] C.-C. Wang and C. Thorpe, “A hierarchical object based representation for simultaneous localization and mapping,” in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Sendai, Japan, September 2004.
[9] F. Lu and E. Milios, “Globally consistent range scan alignment for environment mapping,” Autonomous Robots, vol. 4, no. 4, pp. 333–349, 1997.
