LOCALIZATION AND MAPPING USING A NON-CENTRAL CATADIOPTRIC CAMERA SYSTEM

This work details the development of an indoor navigation and mapping system using a non-central catadioptric omnidirectional camera and its implementation for mobile applications. Omnidirectional catadioptric cameras find their use in navigation and mapping of robotic platforms, owing to their wide field of view. Having a wider field of view, or rather a potential 360 field of view, allows the system to “see and move” more freely in the navigation space. A catadioptric camera system is a low cost system which consists of a mirror and a camera. Any perspective camera can be used. A platform was constructed in order to combine the mirror and a camera to build a catadioptric system. A calibration method was developed in order to obtain the relative position and orientation between the two components so that they can be considered as one monolithic system. The mathematical model for localizing the system was determined using conditions based on the reflective properties of the mirror. The obtained platform positions were then used to map the environment using epipolar geometry. Experiments were performed to test the mathematical models and the achieved location and mapping accuracies of the system. An iterative process of positioning and mapping was applied to determine object coordinates of an indoor environment while navigating the mobile platform. Camera localization and 3D coordinates of object points obtained decimetre level accuracies.


INTRODUCTION
Indoor mapping and navigation, especially using mobile robots, is an ever increasing demand owing to its multiple applications.Many different sensors can be used for indoor mapping and navigation, including laser scanners, odometers, and different vision-based systems.The simplest and the lowest cost systems are cameras.Camera systems that can be used for this purpose are also of a wide variety.They can vary from a single camera to stereo camera to more complex systems such as dioptric and catadioptric systems.Dioptric and catadioptric systems are gaining popularity due to their high field of view.Dioptric cameras are lens based systems.A commonly used dioptric camera is the fish-eye lens camera.Catadioptric cameras are a combination of a conventional camera with a mirror.Most commonly used catadioptric cameras aim to combine mirrors and lenses in such a way as to achieve a 3600 horizontal view (Geyer and Daniilidis, 2003;Ohte et al., 2005;Gaspar and Victor, 1999).Although several configurations of catadioptric cameras are possible, the configuration most useful for this application is a mirror in shape of a quadratic surface, for example a hemisphere, a paraboloid or a hyperboloid, symmetric about the line of the optical axis of the camera.The camera and mirror facing each other thus form an omnidirectional sensor (Fig. 1).The wide field of view leads to further benefits, such as less processing due to the reduced number of images for the same amount of data, and thus resulting in the use of less storage space and higher processing speeds.There are further advantages of catadioptric cameras as compared to a single perspective camera or a stereo camera system, such as the 360 o view, which allows for spatial awareness in all directions without having to turn to view the Figure 1.Concept of the catadioptric camera whole environment or the direction of movement if it is not visible.Catadioptric cameras can be further divided into two types: central and non central.A vision system is said to be central when the optical rays to the viewed objects intersect at a single point in 3D called single effective viewpoint).Central catadioptric camera systems have an advantage over non-central catadioptric systems because of their geometry.Having a single effective viewpoint makes calculations simpler due to additional constraints in geometry.
Another condition is the case when elliptical or hyperbolic mirrors are used, is that the perspective camera is to be placed at one of the focal points of the mirror.Therefore central catadioptric cameras have very specific configurations and, in practice, are very hard to construct.For example, a spherical mirror with a perspective camera is a non central camera or even a hyperbolic mirror with a perspective camera and a misaligned optical axis is a non-central catadioptric camera.In solving for a non-central catadioptric camera, not only is there a requirement to overcome the lack of a single effective viewpoint but also to overcome a misalignment of the optical axis with the mirror.Therefore an additional calibration step is required in the mathematical model.A few of the advantages of the catadioptric systems over other imaging systems are: wider field of view, low cost, light weight, flexibility and portability.

RELATED WORK
Several researchers from around the world have contributed to this field with most of the work focusing on calibration of omnidirectional cameras in order to obtain rectified images (panoramic images viewed as a mosaic of perspective images) from the system (Geyer and Daniilidis, 2000, Scaramuzza et al., 2006, Puig et al., 2001, Aliaga, 2001, Mei and Rives, 2007).Some of that research (Tahri and Araujo, 2012) explored trajectory determination using common detected features and even 3D mapping using incremental bundle adjustment.However the initial and the main step in those studies focused on removing the image distortion caused by the shape of the mirror.The solutions for navigation and mapping using catadioptric cameras can be broadly divided into two categories: a) solutions based on ray optics geometry (Geyer and Daniilidis, 2000;Micusik and Pajdla, 2004;Aliakbarpour et al., 2014;Tommaselli et al., 2014) and b) solutions based on image unwrapping generated from geometric conditions (Aliaga, 2001;Geyer and Daniilidis, 1999).Solutions based on ray-optics use the physical geometry of the catadioptric system, the mirror shape and reflection equations to obtain the respective image coordinates of a physical point (in the object space) observed in the image.Alternatively other solutions are based on feature geometric conditions (e. g., vanishing point geometry) and image unwrapping.

SYSTEM DESCRIPTION
The objective is to develop a catadioptric system for a mobile robot (Fig. 2).Therefore, the specifications used are similar to that of the mobile robot, allowing also additional flexibility in prototyping not only in terms of size but also in terms of mirrors and cameras used.Several mirrors were initially tested, including convex mirrors, mirrors of different sizes and a decision was made to use the current mirror shape based on the field of view, portability and the quality of the images.Another very important physical attribute which favours the use of constructed system rather than the robot for experimentation is the vertical distance between the camera and the mirror.Even though this distance is supposed to be fixed as in the case of the robot, the flexibility in the constructed system allows it to be fixed at the most suitable length, which allows the mirror to be fully viewed in the camera image.Once experiments are performed and a suitable mirror and camera are selected, the same configurations can be implemented on a mobile robot.The use of camera and mirror located on the mobile robot would have limited the experimental process and therefore a flexible prototype system was built and used.
A platform was constructed for the implementation of a catadioptric camera for the purposes of navigation and mapping.The characteristics and specific design parameters of the platform are discussed below followed by the different coordinate systems used in the calibration and positioning of the system.The mathematical model for the calibration of the mirror and camera is described in section 3.1.

Figure 2. Mobile robot
The catadioptric camera used for the purposes of navigation and mapping consists of a quadratic surface shaped mirror facing towards the camera lens (Fig. 3a).The aim is to have the mirror symmetric the optical axis of the camera.In this case a paraboloid mirror fixed facing down to a plate is used.This is attached to a base with the camera on it, facing up.The mirror as viewed from the camera is shown in Figure 3b.The paraboloid mirror can be replaced by hyperbolic, spherical, conical, or elliptical mirrors of varying sizes.The system was constructed to allow for flexibility in selecting: 1. Camera -The system can handle several different cameras.In the configuration used in experiments, the mirror is placed approximately 12 cm distance from the camera.However a flexibility of up to 35 cm is permitted.Since the different cameras use different lenses with different focal lengths and depths of field, this flexibility in positioning of the mirror allows for a wide range of cameras that can be used in this system.
3. Mirror size -The aim was to have flexibility in platform so that it could be used with larger mirrors.The system was constructed to allow mirrors of different sizes (going up to 25 cm in diameter) and offering portability and significant visibility based on the mirror shape.The mirror used has a diameter d p = 78 mm and height h p = 38 mm (Fig. 3c).The mirror has a high vertical field of view of 142.5 o (upwards from the nadir of the mirror) and is noncentral.The mirror is attached to the top plate using a ball screw in such a way that the mirror axis is perpendicular to the plate.The bottom plate, on which the camera is fixed, is not assumed to be parallel to the top plate or mirror base even though attached to the top plate.This flexibility is allowed and the initial calibration process shown in the next section can be used to account for this.However, if such an assumption was made to reduce the number of calculations, it would be a safe one owing to the construction of the system.The distance between the base of the mirror and the top plate is kept fixed.The calibration method is used to calculate the distance between camera lens and the top plate and since the distance between top plate and mirror base can be measured, the distance between camera lens and mirror base, which is the one required for further calculations, can easily be calculated.The top plate can also be moved up and down to allow for the camera to capture the mirror image as best as possible.Therefore, if a mirror with a larger radius is used, a bigger gap between the top plate and bottom plate would be required.Basically, this feature exists in the system to allow for flexibility in the mirror size.

Calibration of Mirror-Camera Systems
The intrinsic geometry of both camera and mirror is shown in Figure 4. Since, the catadioptric camera system consists of two subsystems, the camera and the mirror, the relative linear and angular displacements between camera and mirror reference frames are needed to be determined in order to consider the catadioptric camera as one monolithic system.These are calculated using a calibration process, which is performed between two rigid bodies with six degrees of freedom: rotation and translation expressing the relative position and orientation of the two systems.A local 2D coordinate system, centred at the mirror vertex V, is set up on the plate to which the mirror is attached.Four to eight points are marked as targets in this coordinate system on the top plate.The images of the targets captured by the camera are used to obtain relative orientation of the camera and the mirror.A 2D projective transformation is used to determine the angular and linear displacements.The 2D projection matrix has 8 unknown parameters.The eight unknown parameters exist due to the 6 degrees of freedom (rotation and translation) and 2 intrinsic parameters ( p p y x , ).The projection matrix can be compared to a homogeneous coordinate representation of collinearity equations.This representation includes a scale and intrinsic camera matrix.Since the camera has already been calibrated and its intrinsic parameters (principal points, focal length and lens distortions) are known, the 8 elements of projection matrix (once obtained) can be used to obtain the 6 linear and angular displacement parameters (Khurana, 2016).

NAVIGATION AND MAPPING USING THE CATADIOPTRIC CAMERA SYSTEM
Following the mirror-camera calibration, the position of the catadioptric system in the object space can be determined using control points (CPs), which are targeted points in the object space with known coordinates.This is repeated for several positions of the system.The images captured from these positions are then used to obtain the 3D coordinates of common features forming a sparse point cloud using epipolar geometry.These calculated coordinates can then be used to position the catadioptric camera once it has moved to its next location.
These steps are repeated and constitute a sequential process for navigation and mapping.

Calculation of Mirror Coordinates of the Control Points
As mentioned, the relative angular and linear displacements between the mirror and the camera as obtained from calibration can be used to combine the camera and mirror into a single monolithic system -the catadioptric camera.This is done by establishing a relationship between the image coordinates and the corresponding mirror coordinates of the CP (Figure 4).Essentially, the mirror coordinate of a CP is the intersection between the image ray emanating from the CP and the surface of the mirror.Using the collinearity equations, direction of the optical ray, and the shape of the mirror, as constrain, a one-toone relationship is established between the two sets of coordinates.
The objective is to determine the unknown mirror coordinates ( ) in the catadioptric coordinate system of the CPs, where i refers to the control point.The control point is reflected in the mirror at these mirror coordinates and then recorded in the image.The image coordinates x ai and y ai of a point α ι can be measured and are utilized to obtain mirror coordinates based with the already calculated rotation and translation matrices ) , , ( and ( ) The mirror coordinates are determined from these known parameters using their relationship as described by the collinearity equations and the paraboloid mirror surface equation given as:  ( ) (3) The coordinates of the camera lens C in the catadioptric camera coordinate system are ) , , ( . In the next section C and i M will be used to determine the position of the camera in the world coordinate system or in the local system of the indoor environment.

Positioning of the Catadioptric System
Once the camera catadioptric camera system has been calibrated, it can be used for the purposes of positioning, mapping and navigation.Positioning involves using known coordinates of CP to obtain the coordinates of the catadioptric camera in the world coordinate system.Mapping uses stereo pairs of these catadioptric camera positions to obtain 3D coordinates of common features, such as tie-points (TPs) Navigation can also be termed as "Incremental Positioning" since it uses the same mathematical model as positioning, the only difference being that the control used for the navigation step are the coordinates of tie-points determined from mapping.This is illustrated in Figures 5, 6 and 7, respectively.
In Figure 5, the previously set-up CPs are used to calculate the coordinates of the catadioptric camera at two separate locations.

Figure Positioning
In Figure 6, the two positions are used in stereo-mode to determine coordinates of tie-points in the local environment.In Figure 7, the tie-points, which were determined in the previous step are used to calculate the coordinates of the catadioptric camera at a new location.It should be noted that positioning and navigation (incremental positioning) use a single catadioptric image, whereas mapping requires at least two.
Positioning of the catadioptric camera follows a very similar procedure as space resection.Multiple CPs with known coordinates are used to obtain the 3D coordinates of the catadioptric camera in the world coordinate system.Due to the addition of a mirror, this process is more complex than the space resection using a perspective camera.To account for this complexity, the reflective properties of the mirror are utilized.A detailed methodology for obtaining the 3D coordinates of the

3D Coordinates of the Camera in the World Coordinate System
For the purpose of positioning the catadioptric camera in the world coordinate system, the coordinates of the camera lens, expressed by ) , , ( and the mirror coordinates in are utilized.The coordinates of CPs in world coordinate system are also required.These are described as ) , , ( (Fig. 8).
Figure 8. CP reflection on mirror and imaged on the camera Since the system of the catadioptric camera is to be positioned in the world coordinate system, there are 6 unknown parameters to be determined, namely 3 rotational parameters ) , , ( .These are solved using the mirror's reflective properties.R and T are then used to calculate the camera lens coordinates C′ in world coordinate system, which represents the position of the catadioptric camera as: Similarly, the corresponding mirror coordinates in world coordinate system of the CPs can be determined by equation (5).

T RM M
Since the mirror surface is perfectly reflective, it is known that the incident angle γ between the vector i i M P ′ and the normal vector of the surface on that point Mi n′ is the same as the reflected one between i M C ′ ′ and Mi n′ (Fig. 8).However, the normal vector in the world coordinate system is not known but can be easily derived as the normal vector to the mirror surface at point M ′ (equ.7).Therefore, to obtain the normal vector, the equation of the mirror in the world coordinate system and the coordinates of the point are required.These, however, are yet to be calculated after the rotation and translation matrix are obtained.Therefore the only way to use Mi n′ and the condition of equal angles, is by having Mi n′ as a function of R and T .
The normal vector, Mi n′ , is first calculated in the coordinate system of the catadioptric camera.i N is a point at unit distance along the normal vector from i M (in catadioptric coordinate system).
Using equ. 3 that expresses the shape of the paraboloid mirror, the normal unit vector in the catadioptric coordinate system at the mirror location i M can be represented by equ.6: We recall that i M was calculated using the collinearity equations and equ. 3. The normal vector (in world coordinate system) can be represented by using equ.7: But as described before i M ′ is not known.An alternative way to express Mi n′ is required, where the relationship between the normal vector in the catadioptric coordinate system Mi n  and normal vector in world coordinate system Mi n′ can be described as a function of the rotation matrix as follows: Since, the incident angle γ between the vector Because the three vectors (reflection, incidence and normal) all lie in the same plane, the coplanarity condition can be applied as given in equation 10.
C′ and i M ′ are substituted from equations 4 and 5 in equations 9 and 10 to obtain R and T , respectively.Since there are only two equations and 6 unknowns ) , , , , , ( , at least 3 CPs and the corresponding mirror coordinates are required ( ) 3 ≥ i . More CPs were used to obtain an over-determined solution.There are also restrictions on which CPs can be used based on their geometry.For example, collinear CPs cannot be used since that leads to an ill-conditioned numerical solution.Control points spread out in different directions and at different elevations are preferred.Figure 9 below displays how the catadioptric system is positioned using targeted CPs in an indoor environment.
Figure 9: Indoor Environment These can then be solved together for all unknowns using all points by using least squares adjustment.The initial parameters for each location were approximated as the center of the room with no rotation.It should be noted, however, that least squares works optimally for systems where certain assumptions can be made.One of these assumptions being that X and Y are identically distributed without any bias.In this case, distribution along X and Y is different because of the shape of mirror, i.e. objects in the local environment appear distorted in the image of the mirror.This is the image which is used for measurements.However, least squares solution was still applied with the assumption that such distortions have minimal effect on the results.

Mapping using the Catadioptric System
Once R and T have been calculated, the position of the catadioptric camera in the world coordinate system can be obtained.The position of camera lens, ( ) , and the mirror coordinates of the CPs,

(
) , can also be calculated.Following an identical process for an image taken from a different position, using the same or different targeted points, the position coordinates for the catadioptric camera at a different position can be determined.Since the system settings might change at a different location, there might be a need to repeat calibration and make use of those parameters while determining the new position.Therefore two (or more) images taken from different positions are used to obtain 3D coordinates of common features.This is done by space intersection using coplanarity conditions, where a couple of modifications are made.Firstly, mirror coordinates instead of image coordinates are used and secondly the conjugate rays are the reflection vectors, extending from the reflection of the object in the mirror in the direction of the object.
In order to calculate the 3D coordinates of a point Q (Fig. 10), two calculated positions of the catadioptric camera in the world coordinate system and corresponding images 1 C and 2 C can be used.For this, the image coordinates of the point Q in each of the two images are measured: ( ) in the first image from first position, and in the second image from the second position.The corresponding mirror coordinate for these points can be obtained from the collinearity equations and the paraboloid equation.These corresponding mirror coordinates (in the world coordinate system) are referred as The normal vectors at q M 1 and q M 2 are referred to as q n 1  and q n 2  .The normal vectors can be calculated since the surface of the paraboloid and coordinates of the mirror points are known.Also, since 1 C , 2 C , q M 1 and q M 2 are known, the reflection vector from the each of the mirror coordinates to Q can be determined.This derivation is given next.The direction of a reflection vectors q r 1 and q r 2 can be determined from the incidence vector and the normal vector (de Greve, 2006).
where, r , n  , and i are the reflection, normal unit, and incidence vectors, respectively.
Rearranging equ.11 and introducing the calculated values of the normal vector, mirror and lens coordinates, equations 12 and 13 are obtained: ( ) where, are vectors from the corresponding mirror coordinates of Q to the camera lens at locations 1 C and 2 C respectively, vectors q n 1  and q n 2  are unit normal vectors from the corresponding mirror coordinates of Q at locations 1 and 2 respectively, q r 1 and q r 2 are the vectors from q M 1 and q M 2 respectively in the direction of Q .Vectors q r 1 and q r 2 are the direction rays for Q M q 1 and Q M q 2 .Thus, introducing the unknown scale factors 1 λ and 2 λ : The scale factors 1 λ and 2 λ can be determined by introducing the co-planarity condition between Once 1 λ and 2 λ are determined, the 3D coordinates of the point Q can be calculated as:

Image Measurements
Feature detection and matching is easier in unwrapped images than original catadioptric images (Dupuis et al., 2014, Scaramuzza et al., 2007).When images are unwrapped, they represent a perspective panoramic view.Although the unwrapped images have imperfections and distortions, the characteristics and attributes of features, such as their shape, are more accurately preserved than in the distorted catadioptric images.This is the reason unwrapped images are preferred for feature detection and matching.
In our case vanishing point geometry was used for the image unwrapping.All vertical parallel lines are assumed to intersect at the centre, while all horizontal lines are to be parallel.For example, the vertical ends of the checker paper shown in Figure 11 are parallel.A simple interpolation length-pixel interpolation algorithm was used for the generation of the unwrapped images based on the parallelity of the vertical lines.However, we noticed that the unwrapped panoramic image was slightly distorted especially from top and bottom corners.
The measurement of the image coordinates of CPs and TPs was done manually.Initially automatic image matching was used for measuring the image coordinates of the TPs but this resulted in a large number of mismatches.

Localization Test
The experiment was performed in a room consisting of a straight pathway and three cubicles on each side.Several control points were set up in the room, some along the cubicle wall panels and some inside the cubicle.The mobile platform with the catadioptric camera was moved to five different locations.These locations are denoted as L1, L2, L3, L4 and L5 respectively.Three of these locations (L1, L2 and L4) lie in a straight line along the pathway, whereas L3 is inside one of the cubicles.L5 also lies along the pathway, however at a different alignment than L1, L2 and L4.The coordinates L1, L2, L3 and L4 were determined as described in section 4.2.1 using CP and the coordinates of L5 were determined similarly but using TP derived from images captured at the previous four locations of the mobile platform.

Mapping Test
For the mapping test the 3D coordinates of common tie points captured from two location of the catadioptric system were estimated.Table 2 provides the measured and calculated coordinates of the mapped points along with errors.
When using CP the localization errors are in the level of decimetre, while this deteriorating to about 30 cm when only TP are used.Similarly the mapping accuracy of the TP again is in the decimetre level.Obviously, the distance of the camera system from the targets appears to be the most decisive factor in accuracy of the results.This is also observed when different results for the same location are compared.The results which were obtained using nearby targets are much closer to the reference of position than the ones obtained with farther targets.Distance from targets is not the only factor which impacts the results.It is also observed that if the targets are well distributed in all directions from the mirror, much better results are obtained.However, if they are concentrated on one side, the error is high.This is because it leads to less constraint which results in more freedom of movement and a weak solution.
Other factors contributing to low accuracies are the measurements on a slightly distorted panoramic image and the height of the TPs.The latter is because of the shape of the mirror and because the change in reflection angle is more pronounced at a point reflected higher up on the mirror.The distortion of the target due to its reflection in the paraboloid mirror does make this harder to measure especially since the target is distorted differently at different locations.Therefore points which have a lower elevation are mapped more accurately.This is also true for system positioning, where CPs with lower elevation give a more accurate result for the location.Uncertainty of the mirror shape expressed as paraboloid surface can also contribute to the error budget.

CONCLUDING REMARKS
Mathematical models were developed to evaluate vision based navigation and mapping using catadioptric cameras.This was done in four major steps: calibration, positioning, mapping and navigation.A low cost, highly flexible system was constructed in order to test these mathematical models.In calibration, the relative orientation and translation of the mirror coordinate system and camera coordinate system (the image coordinate system) were determined, that is, their relationship was established so that the system could be treated as a single unit (a catadioptric camera).This was done by using the image coordinates of the targets placed on the mirror mounted plate of the system and then using projective geometry and collinearity conditions.To position the catadioptric camera the mathematical model utilized two conditions, both contingent upon the reflection of the mirror.The first one was the collinearity equations linking the mirror coordinates of the CP, that is the reflection of CP in the mirror as seen in the image, to the CP object coordinates.The second exploited the coplanarity of the three points: mirror coordinate, CP and lens coordinate.At least 3 CP were required to obtain the position of the catadioptric camera.The mirror coordinates of the CPs were determined before this step directly from their respective image coordinates using a collinearity equation and the shape of the paraboloid.These different system set-ups were then used to map points in the room using space intersection based on epipolar geometry.Therefore, a few common points visible in two or more images were mapped.These densified CP were then used to calculate the position of the catadioptric camera when placed at a different location.Therefore navigation involves sequential and simultaneous positioning and mapping similar to incremental bundle adjustment or structure from motion.This process was performed several times in order to represent and simulate the route of a mobile platform on which the system would be placed.Experiments were performed in order to compare these determined positions with positioning results so as to get an idea of the efficiency of the system.Finally the results from each of the stages were obtained.The navigation results as expected were less accurate than the positioning results.They were calculated in the same way using mapped points as targets.The process was evaluated and compared with actual measurements.The camera distance to targets is the main source of error.The positioning error can be calculated in terms of the average distance of targets used, which gives the result as 1/12, which is the ratio of displacement error to the average distance of targets used.Another important factor which affects the results is the number and distribution of CPs used.Generally the more CPs, the better the result, however, there needs to be caution on the CPs used.CPs nearer to the camera and ones which improve the geometry are preferred.
Regarding the image wrapping for image measurements a proposed solution involves making use of both vertical and horizontal lines, which are ideally supposed to be parallel and implementing it along with the previous mathematical transformation.An alternate solution would be performing cylindrical projection using two different cylinders on the two halves of the image (top and bottom).These ideas can be implemented in the future work performed on the system.

Figure 3 :
Figure 3: Constructed system (a: top left), corresponding camera image (b: top right) and mirror dimensions(c: bottom)

Figure 4 .
Figure 4. Mirror-Image Relationship where the shape (curve) of the parabola along x and y axis respectively as function of the paraboloid diameter p d and height p h the vertex V of the mirror is the origin of the catadioptric coordinate system the equation of the paraboloid becomes:

Figure 10 :
Figure 10: Mapping using 2 positions of the catadioptric sensor

Figure 11 :
Figure 11: Unwrapping of the catadioptric image

Table 1 .
Table 1 gives the differences between the measured coordinates with calculated coordinates at each location.Localization results

Table 2 .
Mapping results