Reconstruction 3D of the Geometry and Texture of Environments with a Wheeled Mobile Robot Janelcy Alferes

Cartif Center Technologic Spain

Eduardo Zalama Jaime García_Bermejo Univ. Valladolid Spain

Univ. Valladolid Spain

Abstract In this paper we present a method for the construction of 3D maps from an environment, using a wheeled mobile robot that has onboard a laser scanner to register the geometry of the scene, and a webcam that stores the texture information. The 3D points are projected on planes and subsequently, a triangulation is applied to obtain the surfaces and contours of the model. The images are assigned to the plane surfaces and by means of an stitching algorithm (image mosaics), the complete scene is compose, in “Vrml” format. However, in this procedure there is lost information relative to the free forms that are in the scene, since these are considered points discarded in the model or they can not be approximate to planes. To improve this inconvenience, a variant in the treatment of the data is proposed, that consists in projecting the images and the laser measures on a sphere, with the purpose of diminish the generated distortion. After to obtain the corresponding planes in angular coordinates (teta, phi), the assignment of texture would be realized on the triangular surfaces obtained of the model points.

1 Introduction The generation 3D models from a environment, is a subject that is currently on study, owing to the potential applications that these models, such as: Automatic systems for robot path, surface classification, quality control and faults detection; image segmentation and 3D reconstruction, biomedical applications, architectural design for modeling environments without human intervention. Different methods have been studied for the obtaining of 3D models. However, the traditional techniques do not face to the situations that happen with the mobiles robots and with great environments. The methods based on artificial vision [3],[4],[5],[6], frequently produce ambiguity and errors on the model, because the scene images are not clear. Others methods on study use wheeled mobile robots that has onboard lasers and camera, for register the information [7],[8],[9],[3]. The principal problem is the techniques used for the data treatment and the obtaining of 3D model. Recently, a method for the environments modeling has been development on Cartif using a laser scanner, based on the plane estimations of the scene, by means of statistical information [1]. After, the plane

Gerardo Fernández Univ. Simón Bolívar Venezuela

Juan Grieco

Univ. Simón Bolívar Venezuela

conjuncts are simplified and the surfaces are created with a triangulation of the points in each plane. The scene texture information is provide by images from a camera, and they are assigned to the planes using the method proposed [2]. This algorithm produces lost of information relative to the free forms that are in the scene, so that a method for diminish the objects distortion is introduced in this paper, that is to project the 2D and 3D points on a sphere. The paper is organized as follows. First, the acquisition platform and the modeling method employed will be described. Then, the problem is presented and a method is proposed for improve the results. Finally, the method is applied on some images for to analyze the results and to establish the respective conclusions.

2 Data Acquisition Platform The wheeled mobile robot platform used, is showed on the figure 1. This one has different elements for register the scene information. A laser scanner Sick [10] registers the geometry information. The laser, return the distance “r” of the object on the scene and the correspond angle of sweep “ θ ”. The laser takes up to 180º measures in a same plane and a stepper motor allows that the Sick pans vertically up to 90º to obtain a complete sweeping of the scene. Laser Sick

webcam

Figure 1. Mobile Platform used for the data acquisition.

A webcam registers the scene texture information. A servomotor allows to the webcam mounted on the Sick, turn up to 180º in a same plane, and this way, take the images corresponding to the 3D points taken by the scanner, obtaining a complete sweeping of the scene.

3 Environment Model The algorithm used for to obtain the 3D model is based on [1]. It has been adapted to the characteristics of the wheeled mobile platform and to the elements position continues changes. The algorithm is based in the normal calculation on each point of the sample; this one is associated to the smaller variance. Then, the normal are simplified until obtaining a simple plane conjunct. After, the planes are limited for the application of Delaunay triangulation [11] on the points of each plane, obtaining the surfaces. Finally, the coordinates VRML are created for to build the virtual world. The next step, consist on using the registered information by the camera for to add texture to the group of obtained planes, since that a direct relation exist between the points 3D and the 2D images of the scene [12], if the relation between the optic axis of the camera and the registered points by the laser is known. The employed algorithm is described in [2]. This one undoes the images perspective, assigns the images to the planes and after, a stitching technique (image mosaic) is applying on the conjunct of images for to compose the scene [6][13][14]. The next figure show the results obtained after applying the method on the hall of Cartif.

(a) Frontal view point.

(b) Left Lateral

(c) Right Lateral

Figure 3. View points of the extinguisher.

The reason of the described problem is that the used algorithm assigns the points to the planes. If there is a sample that can not be assigned, the algorithm considers them discarded points. At best, these points are assigned to one plane, but, in any option there is lost information and distortion perspective.

5 Proposed Method

(a). Model with planes

(b) Model with textures

Figure 2. Model 3D of Cartif Hall.

With the finality of to improve the described situation, a variant on the data treatment is proposal, that consists on projecting the images and 3D points on a sphere that is located in the optical center of the camera, since that the camera and the Sick describe spherical movements around a central point. The 3D points of the scene are projected by the camera on a plane image located to “f” distance of the focus. Then, these planes are projected on a sphere [15][16][17] for to obtain the texture of the scene. The figure 4 shows the scheme of projection proposed.

4 Problem When applying the described procedure, efficient results are showed for environments formed by plane surfaces. However, when we navigate for the created virtual scene, we observe lost information relative to the free forms on the scene, for example: chairs, doors, wastebasket, extinguishes, etc. These objects appear embedded in the planes, and there is perspective distortion for different view points. We observe that the view point showed in the figure 3-(a) (view point where the texture was registered), the extinguisher presents few perspective distortion. However, when we watching the object from different view points, we observe that the perspective is distorted, as appears in the figures 3-(b), 3-(c).

Object (x,y,z) Image Plane focal length Sphere

Figure 4. Projection of the 3D and 2D points on the sphere.

The 3D points are triangulate [11] for to parameterize the surface on triangles that can be projected on the sphere. This allows to carry out the direct assignment of the texture 2D and points 3D coordinates, minimizing the distortion. The complete model is obtained when the stitching is carry out on the sphere. As result, we will have a notable improvement on the object perspective. The figure 5 shows the complete scheme of the proposed method for the extinguisher example.

For the scene reconstruction, we have the following information: Objects 3D points (x,y,z), registered by the laser. These points form the scene geometry and 2D images (y,z) registered by the camera onboard on the mobile robot. These images have the chromatic information. It is possible, to establish a correspondence between the 3D and 2D points. We have used the simple “Pinhole Model” [2][12], in which the light ray from the object pass across a hole to the focus. So an image 2D point corresponds with a straight line that passes across the optical axe, and a object 3D point corresponds with only one 2D point. The figure 7 shows the used model.

Object surface triangulate

Image planes projected on the sphere

3D points projected on the sphere

Figure 5. Projection of the 3D and 2D extinguisher points on the sphere.

The plane to sphere projection (figure 6) is a “azimuthal projection” [18][19], where the plane and sphere have a tangency point. An azimuthal projection can not be exact on the measures of angles and surface area at the same time, so is necessary to decide what aspect to the spherical geometry we wish to conserve. In the conformal projection, the angles are conserved, while in the equal-area projection, the surface area is conserved [20][21]. The gnomonic projection is an azimuthal projection, but it does not preserve angles neither areas. However, it has the propriety that the grid straight lines, correspond to the circles on the grid sphere, so there is a direct correspondence between the points. In this projection, there is distortion on the sphere poles. Since that we not have previous information of the scene, we have used the gnomonic projection on the experiments, besides, it has less computational cost.

Figure 7. 3D to 2D Projection Model

Because we have a mobile platform, with independent elements, it is necessary to realize coordinates changes for to construct the planes and to establish the 2D-3D correspondence between the registered points and de images pixels. The figure 8 shows the coordinate system for each platform element.

Figure 8. Coordinate System Representation

So, we have established the following coordinate system: -

-

Figure 6. Plane to Sphere projection on a tangency point

-

-

Robot Coordinate System (OR), on the center of axes motors. Sick step to step motor Coordinate System (OS). Sick Optical Axe Coordinate System (OP). Camera Servomotor Coordinate System Ok . Camera Optical Axe Coordinate System OC. World Coordinate System (OW).

It is necessary to relate the different coordinate system. We have used the “Denavit-Hartenberg” representation [22], where:

P0 =

Relation between the camera servomotor coordinate system (OCA) and the camera optical axe (OC)

P( x3,y3,z3 ) = T45 .P( x4,y4,z4 ) ; T45 =

T01 .P1

P0 : Point on the coordinate system “0”

where: -

T01 : Transformation matrix for to pass from “1” to “0”

cosα − senα 0 xr T;01 = senα cosα 0 yr 0

0

1 0

where: α is the rotation angle around the z’ axe, and

(xr,yr) are the robot translation coordinates on the axes (x,y).

Relation between the robot coordinate system (OR) and the Sick motor coordinate system (OS) cos β − senβ 0 a1 T12 =

0 0 −1 0 senβ cos β 0 b1 where: - β is the rotation angle around the robot z1 axe - (a1,b1) are the vertical and horizontal distances between the robot motor axe and the sick axe

where:

These equations allows to know and to controller the optical axe position and the 3D points, for any moment of wheeled mobile platform elements. The matrix that relates the 3D points with the 2D chromatic points, is obtained by means of a calibration procedure [2][12], since that the optical and the sick axe position is know on the world coordinate system. The image geometry transformation has two important steps: 1.- “Mapping” the original coordinates to the final coordinates [21] for to obtain the new position of the original point. It is to evaluate the “Mapped Function”. 2.-Determination of the intensity value on the resultant image [21]. In this step we have points without intensity value on the final image and/or values of the original image that can not be assigned on the final image. It is necessary to use an interpolation algorithm. Mapped Function

Relation between the sick motor coordinate system (OS) and the sick optical axe coordinate system (OP)

P( x1,y1,z1) = T23 .P( x2,y2,z;2 )

is the camera servomotor rotation angle

between the servomotor axe and the optical axe.

Relation between the world coordinate system (OW) and the robot system (OR)

P( x' , y' ,z' ) = T12 .P( x1, y1,z1 ) ;

− senθ − cosθ 0 − a4senθ

- (a4,b4) are the horizontal and vertical distance

P1 : Point on the coordinate system “1”

P( x , y ,z ) = T01 .P( x' , y' ,z' )

θ

cosθ − senθ 0 a4cosθ 0 0 1 b4

One point on the sphere is defined for the pair (x,y,z) it is showed on figure 9: Z

1 0 0 a2 T23 = 0 0 0 − b 2

z

0 0 1 0

−π / 2 ≤ φ ≤ π / 2

R

0 ≤ θ ≤ 2π

φ

- (a2,b2) are the horizontal and vertical distances between the sick motor axe and the sick optical axe - The P( x 2 , y 2 ,z 2 ) coordinates are related with

θ

X

P( x2,y2,z 2 ) = T34 .P( x3,y3,z3 )

cosβ − senβ 0 − a3 ; T34 = senβ cosβ 0 − b3 0

where: -

β

0

1

−1

0

is the sick motor rotation angle

- (a3,b3) are the horizontal and vertical distances between the motor sick axe and the camera servomotor axe.

y

Y

x

the (r, φ ) coordinates (points registered by the sick)

Relation between the sick motor coordinate system (OS) and the camera servomotor coordinate system (OCA)

r

x = R cos( θ ) cos( φ ) y = R sin( θ ) cos( φ ) z = Rsen( φ )

Figure 9. Coordinates of one point on the sphere

Any point on the sphere, is defined for three parameters: the radius (R), the horizontal sweep angle θ and the vertical sweep angle φ . Now, we have must find the relations between these parameters and the image chromatic points 2D. First, we suppose that the focal axe crosses the image on θ = 0; φ = 0 .

Analyzing the plane xy on the sphere and the image:

Coordinates of a point on plane θ − φ z1cos(θ + αh ) θ = a tan( y1 / R ) − αh ; φ = a tan − αv R

Coordinates of a point on image plane

x1 = R ; y1 = R tan( θ + αh ) ; z1 =

Figure 10. Plane xy

Where:

θ : horizontal sweep angle. φ :vertical sweep angle.

R tan( φ + α v ) cos( θ + α h )

Intensity Value It is necessary to apply an interpolation algorithm for to determinate the intensity level on the final image, using one or several pixels from the original image. We have tried two methods: 1.- Near neighbor: zero order. The intensity value of the resultant image corresponds to value of nearer point on the original image [23]. 2.- Bilinear Interpolation: first order. The intensity value is obtained by the linear combination of the four nearer neighbor on the original image. This interpolation allows better results but it has more computational cost [23].

x: projection of sphere point “P” on the X axe. y: projection of sphere point “P” on the Y axe. r: radius of the circles, generated by P on the plane xy x1: projection of the image point “P1” on the X axe. y1: projection of the image point “P1” on the Y axe. r1: radius of the circles, generated by P1 on the plane xy

Analyzing the plane yz on the sphere and the plane:

Figure 12. Bilinear Interpolation

This interpolation is defined for the following equations: p ( u , v ) = a1 p ( i , j ) + a 2 p ( i , j + 1 ) +

Plane xy for a determinate

θ

a 3 p ( i + 1, j ) + a 4 p ( i + 1, j + 1 ) where:

Figure 11. Plane yz

a1 = ( 1 − du )( 1 − dy ) ; a2 = du( 1 − dv )

a 3 = ( 1 − du )dv

; a 4 = dudv

Where:

y: projection of the sphere point “P” on the Y axe z: projection of the sphere point “P” on the Z axe y1: projection of the image point “P1” on the Y axe. z1: projection of the image point “P1” on the Z axe.

Using the information of both planes, we obtain a functions conjunct of “mapped spatial” to obtain the coordinates of the resultant image. Since the optical axe can move by means of camera servomotor and the sick motor, we must to add the horizontal ( αh ) and vertical ( αv ), vision angles ( αv ), so, we obtain the following general equations:

5

Experimental Results

We have tried the proposed method on different images, in order to study the effect of the generated distortion by the geometrics transformations. Each point on the image plane is considered a pixel with a position inside the image ((coordinates (y,z)) and a RGB intensity value. In the firsts experiments, the used algorithm across each point on the image and applies the “mapped function” in order to obtain the coordinates ( θ ,φ ), of the projection the plane on the sphere. The RGB value on each point to the final image is obtained applying the inverse

“mapped function”, to obtain the original coordinates (y,z) and its RGB value. It is the looked value. Then, we applied a bilinear interpolation in order to obtain the RGB of the points in the final image that do not have original pair (y,z). The figure 13 shows a simple “squared pattern” of 400x300 pixels. It is the image plane to which we applied the transformation. Figure 15. Final Plane ( θ − φ ), with the RGB value.

The following figures show the results obtained when we apply the procedure on real images of Cartif Hall.

Figure 13. Simple Image

The following figure, shows the resultants points of the final image, on coordinates ( θ ,φ ), for different relation between the sphere radius (R) and the image plane size (A). (a) Original Image

(a) R/A=0.5

(b) R/A=1

Figure 14. Plane θ − φ

This way, there is major distortion on the image extremes because de type of the used transformation. Too, the distortion increases when the relation (R/A) diminished. We observed better results for sphere radius greater of equal that image width. Exist pairs ( θ ,φ ) that not have origin coordinates (y,z), and “hollows” are observed on the final images. To improve this convenience and to provide more resolution to the final image, on the next experiments we have used a different algorithm, that consist on to cross the grid discrete ( θ ,φ ) of dimensions associate to the original image size, and after, to apply the “mapping function” and to obtain the RGB value, using the bilinear interpolation. This procedure allows to eliminate the initial “hollows”. The grid discrete separation is other important parameter. It allows to obtain a resolution major on the final image. The figure 16 shows the final result, with the correspondent intensity value. The grid discrete separation is 1º.

(b) plane teta-phi, for R/A =0.5 and a discrete grid of 1º

(c) plane teta-phi, for R/A =1 and a discrete grid of 0.5º

(e). Final Image Plane (y,z). Inverse transformation Figure 16. Cartif Hall.

The following images shoes the results obtained when we apply the procedure to the extinguisher image.

(a) Original Image

(a) R/A =1. A discrete grid of 1º (b) R/A = 1. A discrete grid of 0.5º

To respect, we have proposed a method for to diminish the perspective distortion relative to the free form object. This one consists on to project the 3D points (these points have the scene geometry information) and the 2D points (these points have the texture information) on a spherical surface. The reason of to user a sphere is by the system intrinsic characteristic, since that the Sick and the Camera describe spherical movements around a central point. This geometry transformation has two important steps: the “Spatial Mapping” and the assignment the intensity value on the final image. The results analysis allows to demonstrate that the generated distortion study is associated to important parameters. One of them is the relation between the projection surface radius (R) and the original image size (A). We have verified that for R/A value major that “1”, the distortion is minor, by the spherical transformation behavior. Other important parameter is the final image resolution. When the separation in the discrete grid is major, the resolution is better. The final image is “mapped” in a quarter (1/4) of the sphere surface, so that to obtain a scene complete representation, we have to realize the projection on the correspondent horizontal and vertical angle. The results allow to observe that image perspective is distorted on the extremes. It does a direct correspondence with the 3D points. For the immediate future, we have to realize the sphere parameterization on triangles, after, to project the points on the sphere and to obtain the 3D points teta-phi planes, in order to realize the 3D-2D correspondence and to assign the texture to the scene. The next step is to realize the stitching on the sphere, in order to obtain the final model. Too, we propose the study of the other projections, as the “equal-area projection”, and to evaluate the system behavior.

(d). Final Image Plane (y,z), obtained for the inverse transformation Figure 17.Extinguisher Model.

6

Conclusions and future Works

We have present a method for the Environments 3D Modeling, using the data registered by a laser and a camera, both onboard on a wheeled mobile robot. When we realize the described procedure, we observe efficient results for plane surfaces. However, for the free form object, the results show perspective distortion on the object. The reason of this problem is that the used algorithm for the scene construction treats the points assigning them to planes.

References

[1] Martín, Mariano. Technical Report “Threedimensional Modeling of Indoor Environments with a Mobile Robot”. University of Valladolid, Spain. 2002. [2] Diez, José Javier. Technical Report “Texture incorporation to the navigable 3D Models of the interior of buildings with a mobile robots, endowed with Vision Systems”. University of Valladolid, Spain. 2003. [3] Frintop, Simone; Rome, Erich; Nuchter, Andreas. “An Attentive Multi-modal Laser Eye”. Fraunhofer Institut fur Autonomr Intelligente Systeme, Germany. 2003. [4] Debevec, Paul; Taylor, Camillo; Malik, Jitendra. “Modelling and Rendering Architecture from Photographs: A Hibrid Geometry-and Image-based approach”. University of California.2002.

[5] Bajcsy, R; Kamberova, G; Nocera, L. “3D Reconstruction of Environments for Virtual Reconstruction”. 4th IEEE Workshop on Applications of Computer Vision. 2000. [6] Shum, H; Han, M; Szelinshi. “Interactive Construction of 3D Models from Panoramic Mosaics”. International Conference on Computer Vision and Pattern Recognition (CVPR).1998. [7] Thrun, Sebastian; Burgard, Wolfram; Fox, Dieter. “A Real Time Algorithm for Mobile Robot Mapping with Applications to Multi-Robot and 3D Mapping”. IEEE International Conference on Robotics and Automation. San Francisco. 2000. [8] Andreozzi, L; Barnobi, L; Giuffrida, A; Santagati, C. “New Frontiers to Architectural Survey: Laser Scanner 3D”.University of Catania. 2002. [9] Surmann, Hartmut; Lingemann, Kai; Nuchter, Andreas. “A 3D Laser Range Finger for Autonomous Mobile Robots”. Proceedings of the ISR (International Symposium on 32nd Robotics).2001. [10] “Sick Detected the difference” .www.sick.com [11] Francés, A; Lambán, L; Rubio, A. “Voronoy Diagrams and Digital Topology”. University of Zaragoza. 1998. [12] González, Javier. “Visión por Computador”. Editorial Paraninfo. España. 1999. [13] Shum, Heung; Szalinski, Richard. “Creating Full View Panoramic Image Mosaics and Environment Maps”. International Conference on Computer Vision and Pattern Recognition (CVPR).1998. [14] Lensch, Hendrik, Heidrich, Wolfgang. “A Silhoutte-Based Algorithm for Texture Registration and Stitching”. Graphics Models, 2001. [15] “Introduction to Computer Vision”. CS /ECE 181B. 2003. [16] Tumblin, Jakc. “New Direction: Applying Projections”. CS 395/495-26 Spring 2002. [17] “Project One: Spherical Virtual Image Space” www.missouri.edu/w117a/Course/vision. [18] “Azimuthal Projections”. www.mapleapps.com [19] “Zenithal projection”. www.aips2.nrao.edut [20] Howells, Lee. “The Design of an Automated Data Analysis System for Robotic Telescopes and its Application to be Stars”. 2002. [21] “ENV351 Computer Based Map”. Duke University. 2002. [22] Set, Hutchinson. “Introduction to Robotics”. University of Illinois. [23] Pardiñas, Arturo. “Basic Operations on the Spatial Domain” .2002.