Abiel Aguilar-González

Miguel Arias-Estrada

Dense mapping for monocular-SLAM

Simultaneous Localization and Mapping (SLAM) is the problem of constructing a 3D map while simultaneously keeping track of an agent location within the map. In recent years, work has focused in systems that use a single camera as the only sensing mechanism (monocular-SLAM). 3D reconstruction (map) by monocular-SLAM systems is a point cloud where all points preserve high accuracy and can deliver visual environmental information. However, the maximum number of points in the cloud is limited by the tracked features, this is named "sparse cloud problem". In this work, we propose a new SLAM framework that is robust enough for indoor/outdoor SLAM applications, and at the same time increases the 3D map density. The point cloud density is increased by applying a new feature-tracking/densetracking algorithm in the SLAM formulation. In order to achieve real-time processing, the algorithm is formulated to facilitate a parallel FPGA implementation. Preliminary results show that it is possible to obtain dense mapping (superior to previous work) and accurate camera pose estimation (localization) under several real-world conditions.

I. INTRODUCTION

Simultaneous Localization and Mapping (SLAM) is the problem of constructing a map with respect to an unknown environment while simultaneously keeping track of an agent location within the map. This is used in several applications such as, self-driving cars, unmanned aerial vehicles, autonomous underwater vehicles, planetary rovers and domestic robots [START_REF] Thrun | A system for volumetric robotic mapping of abandoned mines[END_REF][START_REF] Olson | Visual terrain mapping for mars exploration[END_REF][START_REF] Chekhlov | Ninja on a plane: Automatic discovery of physical planes for augmented reality using visual slam[END_REF]. In order to develop a SLAM solution, it is necessary to have sensors that observe the environment and if it is possible, sensors that provide information about the trajectory. There are several sensors available for this purpose (odometers, gyrometers, laser rangefinders, cameras, sonars, etc.). In recent years, work has focused in systems that use cameras as the only sensing mechanism (visual-SLAM). This choice was motivated because nowadays, it is possible to find inexpensive, small and light commercial cameras and they provide visual environmental information that can be exploited to create appearance-based descriptions of the map components [START_REF] Carranza | Efficient monocular slam by using a structure-driven mapping[END_REF]. In most current state of the art, the use of a single camera (monocular-SLAM) instead of binocular/trinocular configurations is the most popular and an active research field [START_REF] Tan | Robust monocular slam in dynamic environments[END_REF][START_REF] Holmes | Monocular slam with conditionally independent split mapping[END_REF][START_REF] Concha | Using superpixels in monocular slam[END_REF]. This is because a single camera avoids addressing the multiple cameras synchronization, interpretation of the different responses of each image sensor to color and luminance, and mechanical alignment between cameras.

A. Monocular-SLAM

The idea of using a single camera has become popular since the emergence of the single camera SLAM or MonoSLAM [START_REF] Davison | Monoslam: Real-time single camera slam[END_REF]. This is due to wide availability of single cameras than a stereo pair, through cell phones, personal digital assistants or personal computers. The monocular approach offers a simple, flexible and economic solution in terms of hardware demand and processing time but present the partially observable problem. The partially observable problem exists where the camera does not provide sufficient information from a simple observation to determine the depth of a point. i.e., a feature-tracking across two or more different viewpoints has to be performed. Then, it is possible to obtain tridimensional information from a single camera only when there is an appropriate separation between the viewpoints.

Several contributions in monocular-SLAM have been reported in the literature [START_REF] Tan | Robust monocular slam in dynamic environments[END_REF][START_REF] Holmes | Monocular slam with conditionally independent split mapping[END_REF][START_REF] Concha | Using superpixels in monocular slam[END_REF], however, there are still many problems such as, low performance under dynamic scenes or sparse point clouds. Most monocular-SLAM systems fail due to accumulated errors while the environment is being explored. These cumulative errors produce inconsistent camera pose estimation and incongruous maps. In general, there are three main problems in the current state of the art: 1. It is assumed that camera movement is smooth and that there will be consistency in the appearance of feature points but in general, this is not true. This assumption is related to the feature extractor and feature-tracking algorithm used in the SLAM formulation. This originates an inaccuracy in the camera pose when it captures images with low texture or that are blurred due to rapid movements of the sensor. 2. Most of researchers assume that the explored environment is static and that they only contain stationary and rigid elements; the majority of environments contain people and objects in motion. If this is not considered, the moving elements will originate false matches and consequently generate unpredictable errors in all the system. 3. The world is visually repetitive. There are several similar textures, such as the architectural elements, foliage and walls of brick or stone. This makes it difficult to track features because there are several similar points that introduce false matches. To solve this inconvenience, typically, only the most dominant features are tracked. Unfortunately, this generates the sparse cloud problem that limits the 3D environmental understanding and makes difficult the recognition of objects or places.

B. The sparse cloud problem

Traditional monocular-SLAM systems extract feature points by applying any type of feature extractor [START_REF] Fuentes-Pacheco | Visual simultaneous localization and mapping: a survey[END_REF]. There are several feature extraction algorithms in the literature (Harris [10], FAST [START_REF] Rosten | Fusing points and lines for high performance tracking[END_REF], SUSAN [START_REF] Smith | Susana new approach to low level image processing[END_REF], Shi-Tomasi [START_REF] Shi | Good features to track[END_REF], etc.); however, in all cases the maximal number of features that can be extracted varies between 0.5% and 4.0% of all points in the image, depending on the selected algorithm and its particular configuration. In practice, traditional monocular-SLAM systems work with configurations that allow extracting near 1% of the points from an image [START_REF] Fuentes-Pacheco | Visual simultaneous localization and mapping: a survey[END_REF]. This limits the real-world applications performance since only 1% of the image points is used to obtain the 3D information, thus, the visual environmental understanding, high-level descriptors application and objects/structures recognition in the point cloud have low stability under real-world scenarios. Even the most current and popular monocular-SLAM systems, for example ORB-SLAM [START_REF] Mur-Artal | Orb-slam: a versatile and accurate monocular slam system[END_REF] or LSD-SLAM [START_REF] Engel | Lsd-slam: Largescale direct monocular slam[END_REF] are limited to sparse point clouds.

C. Motivation and scope

The sparse point cloud problem limits the spatial resolution, and therefore, the level of detail in the 3D map. Several SLAM applications such as, automatic car piloting [START_REF] Thrun | Stanley: The robot that won the darpa grand challenge[END_REF], rescue tasks for high-risk environments [START_REF] Thrun | A system for volumetric robotic mapping of abandoned mines[END_REF] and planetary exploration [START_REF] Olson | Visual terrain mapping for mars exploration[END_REF] have difficulties to use the map generated by the monocular-SLAM systems as 3D reference. Typically, 3D reconstruction algorithms carry out the 3D map estimation [START_REF] He | Autonomous navigation based on unscentedfastslam using particle swarm optimization for autonomous underwater vehicles[END_REF]. In recent years, the increase of computing power and a better SLAM algorithms understanding, allowed to increase the point clouds density. In some works, several visual-feature types are bundled in a SLAM framework that increases the cloud density. Other trend that enables semi-dense reconstructions consist in camera localization by optimizing directly over image pixel intensities. Unfortunately, in all cases, the proposed solutions only allow semi-dense point clouds and the performance over untextured regions is low. Furthermore, to reach real-time processing, only low-resolution images are supported. In this work, we are interested in how to increase the density of the monocular-SLAM systems based on RGB cameras. Any RGB-based monocular-SLAM that provides dense mapping would offer full 3D environmental understanding and simplify the high-level descriptors application and objects/structures recognition within the map. In addition, RGB-based monocular-SLAM systems can work correctly under indoor/outdoor environments. Finally, a sparse cloud problem solution would enable the use of monocular-SLAM under several outdoor applications such as, augmented reality [START_REF] Chekhlov | Ninja on a plane: Automatic discovery of physical planes for augmented reality using visual slam[END_REF] or 3D reconstruction [START_REF] Johnson-Roberson | Generation and visualization of largescale three-dimensional reconstructions from underwater robotic surveys[END_REF]. In the current state of the art, the sparse cloud problem is addressed with RGB-based monocular-SLAM approaches. Unfortunately, these approaches are limited to indoor applications in which the objects distributions allow relatively high performance for the depth sensor.

II. RELATED WORK

The authors of the LSD-SLAM algorithm [START_REF] Engel | Lsd-slam: Largescale direct monocular slam[END_REF] proposed skip the keypoint extraction and the feature tracking steps. Instead, it is demonstrated that an area-matching methodology carried out by a probabilistic model allows improving the quality of the generated map. It was introduced a direct monocular approach that simplified the traditional SLAM formulation and at the same time increases the 3D map density. Unfortunately, the solution is limited to semi-dense clouds. In addition, to reach real-time processing, only low-resolution input images are supported. The DT-SLAM algorithm [START_REF] Herrera | Dt-slam: Deferred triangulation for robust slam[END_REF] addresses the problem via deferred triangulation. The authors introduce a real-time monocular-SLAM system that incrementally tracks individual 2D features and estimates the camera pose by using a 2D feature matching. In parallel, 3D features obtained previously by the projective transformation are triangulated and introduced in the current map. Although 3D features increment the density of the map and provide more elements for the current camera estimation, the increase of the map density is low and the developed method only works under indoor workspaces. Finally, the same authors of ORB-SLAM [START_REF] Mur-Artal | Orb-slam: a versatile and accurate monocular slam system[END_REF] addressed the problem using a post-processing step that introduces a probabilistic semi-dense processor. The proposed algorithm was applied over an ORB-SLAM system. It allows compact system design, high resolution in the camera localization, semi-dense mapping and RGB-based point clouds. The main limitations are the low performance under untextured regions and only near 4% input image points are included in the point cloud.

III. THE PROPOSED ALGORITHM

In Fig. 1 an overview of our algorithm is shown. The algorithm is based on how to increase the 3D map density and at the same time allow real-time processing. Considering that increasing the map density is an exhaustive task that implies high computational demand, we formulate our algorithms in such a way it facilitates hardware acceleration. Several types of local features can be extracted (edges, blobs, corners, etc.) and several types of feature extraction algorithms can be used. In the case of monocular-SLAM, there is a tendency of using corners as the unique local feature considered in the SLAM formulation [START_REF] Fuentes-Pacheco | Visual simultaneous localization and mapping: a survey[END_REF]. A corner can be defined as a point, whose immediate neighborhood contains two or more dominant edges of different directions. Several detectors, for example SUSAN [START_REF] Smith | Susana new approach to low level image processing[END_REF], Harris [START_REF] Harris | A combined corner and edge detector[END_REF] or Shi-Tomasi [START_REF] Shi | Good features to track[END_REF] have been used in SLAM formulations, however, they are computationally intensive and the processing time is high. In recent years, the Features from Accelerated Segment Test (FAST) [START_REF] Rosten | Fusing points and lines for high performance tracking[END_REF], has become the most used corner detection algorithm because it enables low processing time and low computational demand and it is what we will use in this work. The original FAST-N algorithm compares the intensity of a corner candidate c with each point from a 16-point circle that surrounds c, as shown in Fig. 2. Then, a feature is detected at c if the grayscale intensities of at least G contiguous pixels are all above or all below the intensity of c by some threshold, t. For more details see [START_REF] Rosten | Fusing points and lines for high performance tracking[END_REF].

B. Feature tracking

In the case of monocular-SLAM formulations, two types of feature-tracking algorithms can be used: feature-tracking algorithms based on two viewpoints and feature tracking algorithms based on frame-by-frame tracking. For algorithms based on two viewpoints, they allow feature matching along large trajectories with a relatively low processing time [START_REF] Rublee | Orb: an efficient alternative to sift or surf[END_REF][START_REF] Bay | Surf: Speeded up robust features[END_REF]. However, the image degradation between viewpoints introduces data inconsistencies that introduce erroneous matches. On the other hand, algorithms based on frame by frame tracking deliver high accuracy without data inconsistencies, but processing time is high and hardware acceleration is complex [START_REF] Lucas | An iterative image registration technique with an application to stereo vision[END_REF][START_REF] Tomasi | Detection and tracking of point features[END_REF]. In this work, we are interested to increase the 3D map density in monocular-SLAM systems based on RGB cameras. Dense mapping requires large number of feature points that are being tracked. Considering that, all feature tracking algorithms in the literature present important limitations since they have wrong matches for algorithms based on two viewpoints and low speed processing for algorithms based on frame-by-frame tracking, in this work, we propose a new feature-tracking algorithm that searches for a region centered in any feature g in frame i that is similar or equal than a similar size region in frame i + 1, located within a search region in frame i + 1. Similar to KL [START_REF] Lucas | An iterative image registration technique with an application to stereo vision[END_REF] algorithm, we propose search regions. But, instead of rectangular patches, we propose circular patches that minimize the inconsistent pixels introduced by rotational motion, this is illustrated in Fig. 2. We define the radius size (r) as shown in Eq. 1, where r is an integer number that represent the radius size and σ is computed as shown in Eq. 2, where X is the horizontal resolution for the input images measured in pixels while Y is the vertical resolution.

r = min{k ∈ Z | ρ ≤ k} (1) σ = X + Y 4 • √ X + Y (2)
Then, we construct a rectangular patch defined as G = 0 2•r+1,2•r+1 . In this case, rectangular patches provide straightforward implementation in FPGA, since rectangular patches can be mapped into RAM buffers with relatively low memory management complexity. After the rectangular patch is computed, we construct a circular region within the patch, this is computed as shown in Eq. 3, where G is a rectangular patch and i k,θ , j k,θ are computed as shown in Eq. 4, 5 respectively. r is the radius computed by Eq. 1 and α is a barrier value defined by the user. Note that Eq. 4, 5 is a modified vectored circumference equation that constructs circular perimeters defined by integer values. Combining Eq. 4 and 5 with Eq. 3 a circular with region radius r can be constructed.

G = 1 ∀ G a k,θ ,b k,θ (3)
i k,θ = r k=0 α θ=0 min{ϑ ∈ Z|(k + cos(2π • θ/α)) ≤ ϑ} (4) j k,θ = r k=0 α θ=0 min{ϑ ∈ Z|(k + sin(2π • θ/α)) ≤ ϑ} (5)
Finally, we can assume that a feature point q in frame i have a displacement in frame i + 1 less or equal than r. Therefore, we can construct one circular patch over a feature point q with coordinates (x,y) in frame i. Then, we can construct n circular patches in frame i + 1, in this case, the n region centers are all points within the search region (circle with radius r centered in coordinates (x, y)) in frame i, see Fig. 4. Finally, by applying any type of correlation function, we can assume that n region center that minimizes or maximizes the correlation function is the tracked position of the feature point q in frame i + 1.

Fig. 4: The proposed feature-tracking algorithm formulation. For each feature point, n overlapped patches are constructed in frame i + 1, n patches centers are all points within the circular patch created in frame i. n region center that minimizes or maximizes a correlation function is the tracked position of the feature point q in frame i + 1.

The sum of squared differences (SAD) has low mathematical complexity and enables simple FPGA or GPU implementation. Therefore, we propose a SAD correlation function adapted to our circular patches. In this scenario, we propose Eq. 6 and 7; where x i+1 (h), y i+1 (h) are the spatial locations for all the g features in frame i + 1. SAD measures the similarity between patches by applying the sum of squared differences, as shown in Eq. 8, where r is the search region size, computed via Eq. 1, considering X, Y as the horizontal, vertical resolution of the input images. I i and I i+1 represent the 2D spatial coordinates of pixels for two different frames from an image sequence, and β, σ are the spatial location for all the patches in frame i + 1, they ranges between r up to r with increments of 1.

x i+1 (h) = g h=1 min β SAD(β, σ) (6)
y i+1 (h) = g h=1 min σ SAD(β, σ) (7)
G(u + r + 1, v + r + 1) • (8)
|(I i (x + u, y + v) -I i+1 (x + u, y + v))|

C. Simultaneous localization and mapping

Given two corresponding point sets: q = {(x 1 , y 1 ), (x 2 , y 2 )

• • • (x n , y n )}, g = {(x 1 , y 1 ), (x 2 , y 2 ) • • • (x n , y n )},
the fundamental matrix F can be computed by applying camera geometry analysis. Then, the essential matrix E can be estimated as E = K F K, where K is the calibration matrix for the camara used in the SLAM formulation. By applied singular value decomposition (SV D) over E and solving for a close solution, the camera matrix P can be estimated. For more details about camera matrix estimation see [START_REF] Hartley | Multiple view geometry in computer vision[END_REF]. Using the P matrix and the corresponding point sets, the Least Squares algorithm [START_REF] Conley | Computer optimization techniques[END_REF] estimates the translation t and rotation R that minimizes the sum of the squared re-projection error. Then, using again q, g and considering the P matrix, any linear triangulation method can estimate the 3D position of all tracked points. Finally, any optimization technique such as, bundle adjustment or LevenbergMarquardt modifications can be used to simultaneously refine the camera pose (t, R) and the 3D positions. In this work, we use bundle adjustment as optimization technique since this has proved successful in the past and is the most common optimization technique used in current monocular-SLAM systems.

D. Dense tracking

In order to reach dense tracking, we propose a modification of our feature tracking algorithm (Section III-B). In general, considering A as a input image sequence, we consider g as all points from an image within the sequence, defined as x i (g) = x, y i (g) = y; where x, y are the 2D spatial position for all points/pixels in the image. Then, we can track all points similar to the Section III-B formulation. But, in this case, we propose eliminating any point correspondence if there are two or more different indexes (β, σ) that minimize the correlation function (Eq. 8). This is because we assumed that if there are two or more different indexes that minimize the correlation function, the tracked point is within an untextured region. i.e., any point/pixel within the search region in the frame i + 1 minimizes the cost function, thus, estimate the position in frame i+1 is impossible if only color comparisons are applied in the correlation function.

E. Triangulation

Given two corresponding point sets:

q = {(x 1 , y 1 ), (x 2 , y 2 ) • • • (x n , y n )},, g = {(x 1 , y 1 ), (x 2 , y 2 ) • • • (x n , y n )},
and considering (P 1 , P 2 ) as the camera matrix estimated via the Essential matrix and camera matrix centered at the origin, respectively, we can compute the following variables:

A 1 = q(x 1 ) • P 1 (3, i) -P 1 (1, i), A 2 = q(y 1 )•P 1 (3, i)-P 1 (1, i), A 3 = q(x 1 )•P 2 (3, i)-P 2 (1, i), A 3 = q(y 1 )•P 2 (3, i)-P 2 (1, i), A[A 1 A 2 A 3 A 4 ]. Finally, let [U S V ]
denote the singular value decomposition (SV D) over A, the 3D positions for all the tracked points (the mapping solution of the SLAM process) can be denoted as V (i, 4). For more details about linear triangulation see [START_REF] Hartley | Multiple view geometry in computer vision[END_REF].

IV. RESULTS AND DISCUSSION

In this section preliminary results for our SLAM formulation are presented. For that, first we present experimental results for our feature-tracking algorithm. We compare the performance for several feature-tracking algorithms previously used in SLAM formulations. Second, we show experimental results for our dense-tracking algorithm.

A. The feature tracking algorithm In Fig. 5, feature tracking performance by applying our algorithm is shown. In these tests, we tracked features obtained via the FAST algorithm [START_REF] Rosten | Fusing points and lines for high performance tracking[END_REF] with the following frame of a video sequence. In order to measure the performance of our feature tracking algorithm, we implemented several feature-tracking algorithms suitable for SLAM formulations via Matlab scripts. Then, we track feature points extracted via the FAST algorithm across 16 consecutive frames from eight different video sequences, respectively. In TABLE I, accuracy comparisons are shown, we present the mean accuracy for all the compared algorithms. In the case of the SURF/ORB [START_REF] Rublee | Orb: an efficient alternative to sift or surf[END_REF][START_REF] Bay | Surf: Speeded up robust features[END_REF] algorithms, they allow feature matching along large trajectories with a relatively low processing time, but, the image degradation between viewpoints introduces data inconsistences that introduce erroneous matches. In order to achieve accuracy required by SLAM applications, statistically robust methods like Random Sample Consensus (RANSAC) have to be implemented to filter erroneous matches. Our algorithm allows high accuracy, superior to SURF/ORB, suitable for SLAM applications and without any filter or post-processing step. The only limitation of our algorithm compared with SURF/ORB is that a frame by frame feature-tracking approach is an exhaustive task. Then, the processing time have to be higher than two viewpoint approaches. This can be observed in TABLE II, were our algorithm and the KLT algorithm [START_REF] Lucas | An iterative image registration technique with an application to stereo vision[END_REF], both frame by frame feature tracking algorithms, have higher processing time than SURF/ORB. Nevertheless, our algorithm has an important advantage because its mathematical formulation allows simple FPGA implementation, suitable for real-time processing. Considering that previous work do not allow simple hardware implementation or do not provide accuracy suitable for SLAM applications, our feature matching approach can provide a convenient framework for SLAM formulations since it enables high accuracy and allows simple FPGA implementation for real-time processing. [START_REF] Group | Rgb-d slam dataset and benchmark[END_REF]).

978- 1 -

 1 5090-2425-4/16/$31.00 c 2016 IEEE

Fig. 1 :

 1 Fig. 1: The proposed algorithm

Fig. 2 :

 2 Fig.2: Relations of the center pixel c and its surrounding pixels in the FAST algorithm. Image taken from[START_REF] Rosten | Fusing points and lines for high performance tracking[END_REF]

Fig. 3 :

 3 Fig. 3: Circular patches vs. rectangular patches. When a rectangular patch in an image A (top-left) is compared with a same size rectangular patch in an image B (top-right), rotation motion introduce inconsistent values (these are represented as white pixels within the rectangular patch). If a circular patches instead of rectangular is used (bottom-left), any inconsistent data are avoided. This can be observed in bottom-right in which pixels within the circle are the same those pixels within circle shown in the bottom-left of the image.

SAD

Fig. 5 :

 5 Fig. 5: Feature-tracking algorithm applied over two consecutive frames form an outdoor video sequence

TABLE I :

 I Accuracy of feature-tracking algorithms used in SLAM formulations (errors are measured in pixels, all sequences were obtained from

B. Dense tracking

In Fig. 5, dense tracking performance by applying our algorithm is shown. In this case, the lines (correspondences between points) overlap in most the image. This is because instead of feature tracking in which only corners are tracked, in dense tracking, all points in the images have to be tracked, therefore, there have to be one line that connected any point in the first image with its corresponding point in the second image.

C. monocular-SLAM

In order to validate the performance, we tested via Matlab several indoor/outdoor video sequences. Although there are several datasets, in most cases several environmental restrictions such as, controlled illumination, uniform camera movements and less image degradation between frames are considered. Environmental restrictions limit the real-world applications performance since these restrictions often are not present. In this work, all video sequences were captured in the interior/exterior of our campus. Then, it is demonstrated that the proposed solution is capable to work in satisfactory form under several real-world conditions.

In Fig. 7 an overview of some tested scenes are shown while in Fig. 8 and9, we present the results for our monocular-SLAM algorithm compared with a traditional monocular-SLAM approach. For that, we formulate a traditional monocular-SLAM approach using the FAST algorithm [START_REF] Rosten | Fusing points and lines for high performance tracking[END_REF] in the feature extraction step, the BRIEF visual descriptor [START_REF] Calonder | Brief: Binary robust independent elementary features[END_REF] in the visual descriptor step, a traditional all vs. all matching algorithm, the normalized eight-point algorithm for the camera pose estimation [START_REF] Conley | Computer optimization techniques[END_REF], the linear triangulation algorithm for the 3D estimation and bundle adjustment to simultaneously refine camera pose and 3D points. As can be seen, in all cases the point cloud density can be increased by applying our monocular-SLAM algorithm. Furthermore, we demonstrated that the proposed algorithm works successfully under different environmental conditions, different places and different camera movements. Therefore, the proposed solution addressed in satisfactory form the sparse cloud problem. In TABLE III we present numerical comparisons between the traditional and proposed monocular-SLAM approach. In all cases high accuracy in the camera pose estimation can be achieved. In all cases, the proposed dense tracking algorithm allows increasing the point cloud density. Considering that previous work that addressed the sparse cloud problem using a single RGB camera, such as DT-SLAM [START_REF] Herrera | Dt-slam: Deferred triangulation for robust slam[END_REF], LSD-SLAM [START_REF] Engel | Lsd-slam: Largescale direct monocular slam[END_REF] and ORB-SLAM [START_REF] Mur-Artal | Orb-slam: a versatile and accurate monocular slam system[END_REF], only include near to 2, 7 and 4 percent of the points from a scene, respectively. Then, our algorithm that can include a mean of 14 percent of the points form a scene, improves the current state of the art, providing dense mapping and accurate camera pose estimation under several real-world indoor/outdoor scenarios. Finally, all exhaustive algorithms such as, feature extraction, feature tracking and dense tracking allow straightforward implementation in FPGA. In the case of the feature/dense tracking algorithms, all pixels/patches can be compared in parallel. This could allow real-time processing that increases the scope of our monocular-SLAM algorithm.

V. CONCLUSIONS

In this article, we introduced a new feature-tracking/densetracking algorithm, suitable for increasing the density of the monocular-SLAM systems based in RGB cameras, and that allow simple hardware implementation for real-time processing. Our SLAM formulation delivers accurate camera pose and dense mapping, superior to previous work. We have demonstrated that our SLAM formulation can be successfully applied under several real-world scenarios. Finally, based on the mathematical formulation of all our algorithms, we can affirm that a simple and compact FPGA implementation can be implemented; this will enable real-time processing with low cost and compact system design.


Abiel Aguilar-González

Miguel Arias
email: ariasmo@inaoep.mx

Miguel Arias-Estrada

Towards a smart camera for monocular SLAM

Keywords: SLAM, SoC, FPGA

come

Towards a smart camera for monocular SLAM

INTRODUCTION

Smart cameras integrate processing close to the image sensor, so they can deliver high level information to a host computer/robot or high level decision process. Progress in microprocessor power and FPGA technology makes feasible to build compact and low cost smart cameras, but the actual problem is how to program them efficiently given the wide variety of algorithms and the custom approach to have a smart camera for a specific application, i.e., for a smart video surveillance the camera could be detecting and tracking pedestrians, but for a robotic camera, the accelerated processing in the camera could be edge and feature detection. Commercially some approaches simply offer a path to recompile an application developed in OpenCV [START_REF] Opencv | Open source Computer Vision[END_REF] or any other programming environment, into an embedded processor, compact x86 compatible processor or DSP processor [START_REF] Batlle | A new FPGA/DSP-based parallel architecture for real-time image processing[END_REF][START_REF] Pulli | Real-time computer vision with OpenCV[END_REF]. Some companies offer a dedicated IP framework coupled with vision processors that accelerate a subset of vision algorithms, mainly motivated by automotive industry trends in visual assistance and a path for self-guided vehicles technology [33].

A recent trend is the integration of GPU embedded processors in platforms that already support a subset of OpenCV like the Jetson TK1 and XT1 boards [START_REF] Rud | Development of GPU-accelerated localization system for autonomous mobile robot[END_REF][START_REF] Vega | Resilient, UAVembedded real-time computing[END_REF]. Nevertheless, an FPGA based smart camera offer advantages from all the previous approaches: possibility to design parallel architectures that accelerate processing time, integration of hardware-software architectures with the SoC FPGAs, low power consumption for embedded applications and lower cost than GPU based embedded solutions. The inconvenient to develop an FPGA smart camera is the lack of standards to reuse FPGA components, lack of internal standard or architecture to facilitate the hardware/software co-processing and effective use of memory transfers between all the submodules in the vision processing pipeline. The latest is being addressed with high level languages and component reuse similar to what is done in software. In particular, solutions like CAPH [START_REF] Sérot | Caph: A language for implementing stream-processing applications on FPGAs[END_REF], GPstudio [START_REF] Gpstudio | [END_REF] or OpenCL libraries [START_REF]OpenCL (Open Computing Language[END_REF] and vision pipeline standards like OpenVX [9] are paving the road to a standard that can help build more sophisticated vision processing pipelines.

Monocular-SLAM in a smart camera

Several SLAM solutions such as, EKF-based, graph-based and visual-based solutions are available in the literature. However, the recent trend is for visual-based solutions, more specifically, monocular-SLAM (visual-SLAM with a single camera), since it can provides visual information such as texture and color from the scene and requires lowest power and cost than other visual-SLAM formulations (stereo-based or RGBD-based solutions) [START_REF] Fuentes-Pacheco | Visual simultaneous localization and mapping: a survey[END_REF]. The basis of monocular-SLAM is that a single moving camera, can obtain 3D information of the environment and deliver a rough 3D map that can include texture and color of the elements within the map. This is highly used in robotics or autonomous vehicle applications since it is possible to navigate and at the same time reconstruct in 3D the robot/vehicle positions and the position of objects, obstacles, walls, etc., in its surroundings [START_REF] Kong | Visionbased autonomous landing system for unmanned aerial vehicle: A survey[END_REF][START_REF] Lee | Slam with sc-phd filters: An underwater vehicle application[END_REF].

Having a smart camera that delivers monocular-SLAM can open several research lines and applications, since at a higher level it will be possible to integrate cooperative information from several cameras, integrate other image understanding algorithms and have a better visual representation of the world. Smart cameras with monocular-SLAM would be useful in autonomous vehicles, drones and mobile robotics, leveraging the central processors of those platforms for the high computational cost of the control and navigation tasks. The preferred choice for an FPGA-based smart camera is to integrate low level image preprocessing tasks and deliver the results to a software processor that performs high level processing tasks. This approach has proved successful in the past [START_REF] Bourrasset | Distributed FPGA-based smart camera architecture for computer vision applications[END_REF][START_REF] Birem | Dreamcam: A modular fpga-based smart camera architecture[END_REF], and it is what we will follow. In previous work only the feature-extractor/feature-matching algorithm was implemented in the FPGA, while the rest of the data processing is carried on in a conventional processor. In our case we explore further integration of monocular-SLAM formulation into the FPGA accelerated architecture.

Related work

Since the last decade, published articles reflect a tendency for using vision as the only external sensorial perception system to solve the SLAM problem [START_REF] Engel | LSD-SLAM: Large-scale direct monocular SLAM[END_REF][START_REF] Mur-Artal | ORB-SLAM: a versatile and accurate monocular SLAM system[END_REF][START_REF] Herrera | DT-SLAM: Deferred Triangulation for Robust SLAM[END_REF]. In some cases, algorithms were formulated in the context of a sensor or smart camera. Authors of [START_REF] Spampinato | An embedded stereo vision module for industrial vehicles automation[END_REF] introduced an embedded vision sensor based on reconfigurable hardware (FPGA) to perform stereo image processing and 3D mapping for sparse features. It was proposed an EKF based visual SLAM. The system uses vision as the only source of information and achieves a convenient performance for small industrial environments. Unfortunately, the approach is limited to sparse 3D maps and, the stereo configuration introduces some inconvenient due to the cameras synchronization and mechanical alignment. In [START_REF] Nikolic | A synchronized visualinertial sensor system with FPGA pre-processing for accurate real-time SLAM[END_REF] a visual-inertial sensor unit for robust SLAM capabilities is presented. Four cameras are interfaced through an ARM/FPGA design, an Inertial Measurement Unit (IMU) provides gyro and accelerometer measurements. The proposed approach delivers a convenient fusion of visual and inertial cues with a level of robustness and accuracy difficult to achieve with purely visual-SLAM systems. The main limitation of the approach is that only the feature extraction algorithm was accelerated in the FPGA, this represents an inefficient hardware/software partition since other tasks such as, feature-matching can be accelerated in the FPGA.

In [START_REF] Mattoccia | A Real Time 3D Sensor for Smart Cameras[END_REF], the architecture and the processing pipeline of a smart camera suited for real time applications is discussed. The authors proposed a memoryless computing architecture based on low cost FPGA devices. It was proposed a stereo matching approach with sub-pixel accuracy. Finally, the results are delivered via USB2.0 front end. The developed sensor allows to infer, dense and accurate depth maps under indoor/outdoor environments. The developed camera was used in a SLAM application, nevertheless, all the SLAM process is carried out in a CPU implementation. This limits the performance for robotics mobile applications in which compact systems with low power consumption are required. In [START_REF] Bonato | A smart camera with gesture recognition and SLAM capabilities for mobile robots[END_REF] a smart camera for a real-time gesture recognition system was presented. The smart camera was designed and implemented in a System-on-a-Chip (SoC) device, using reconfigurable computing technology. In this system, the gesture images are captured by a CMOS digital camera. After some preprocessing steps, images are sent to a Fault Tolerant Module (FTM) for the actual recognition process. The FTM implements a RAM-based neural network, using three knowledge bases. A real-world application was presented, it consists of four smart cameras used in SLAM tasks for robotic navigation. The proposed system aims to increase the accuracy of the maps generated by the SLAM algorithm by using images taken from the robot perimeter. Unfortunately, the algorithm is limited to landmarks, and the 3D maps are sparse maps that limits the environmental understanding.

THE PROPOSED ALGORITHM

In this work, we are interested in a smart camera that delivers a SLAM solution without post-processing steps and that allows a relatively simple and compact system design. In Fig. 1 an overview of our algorithm is shown. We accelerate the feature extraction and feature matching in hardware while the camera matrix/camera pose estimation and 3D estimation are implemented in software. The hardware/software partition is based on what parts of the algorithm can be parallelized, but also targeting that the software part can be executed in real-time in a processor in a SoC FPGA device. Therefore, the software computational load must be low.

Feature extraction

Several feature extraction algorithm have been reported in the literature, nevertheless, in most cases, performance for FPGA implementation is limited. In this work, we present a new feature extraction algorithm that uses the maximum Eigenvalues as corner metric response. This algorithm is robust enough to detect feature points under any types of input images and enables efficient FPGA implementation. Considering 𝐼 as a grayscale image, first, we propose to compute the 𝑥, y gradients as shown in Eq. 1 and 2, respectively.

𝐺 𝑥 (𝑖, 𝑗) = (𝐼(𝑖 -1, 𝑗)) -(𝐼(𝑖 + 1, 𝑗)) (1)
𝐺 𝑦 (𝑖, 𝑗) = (𝐼(𝑖, 𝑗 -1)) -(𝐼(𝑖, 𝑗 + 1)) (2)
We define the 𝐴, 𝐵, 𝐶 matrixes as 𝐴(𝑖, 𝑗) = 𝐺 𝑥 (𝑖, 𝑗) • 𝐺 𝑥 (𝑖, 𝑗), 𝐵(𝑖, 𝑗) = 𝐺 𝑦 (𝑖, 𝑗) • 𝐺 𝑦 (𝑖, 𝑗) and 𝐶 (𝑖, 𝑗) = 𝐴(𝑖, 𝑗) • 𝐵(𝑖, 𝑗). Once the 𝐴, 𝐵, 𝐶 matrixes are computed, they have to be convolved with an appropriate Gaussian kernel. In [START_REF] Aguilar-González | An FPGA 2Dconvolution unit based on the CAPH language[END_REF] we presented an image convolution framework that allows flexible 2D convolution and at the same time high performance for FPGA implementation. Based on our algorithm presented in [START_REF] Aguilar-González | An FPGA 2Dconvolution unit based on the CAPH language[END_REF], we proposed a convolution kernel as shown in Eq. 3. Then, we convolve the 𝐴, 𝐵, 𝐶 matrixes as following: 𝐴 = 𝐴 * 𝑀, 𝐵 = 𝐵 * 𝑀, 𝐶 = 𝐶 * 𝑀, where the operator * represents the 2D spatial convolution between an image 𝐼 and convolution kernel 𝑀. For more details see [START_REF] Aguilar-González | An FPGA 2Dconvolution unit based on the CAPH language[END_REF].

𝑀 = [ 0 0 1 128 0 0 0 1 128 1 64 1 128 0 1 128 1 64 1 8 1 64 1 128 0 1 128 1 64 1 128 0 0 0 1 128 0 0 ] 𝑀 = [ 2 0 2 0 2 0 2 0 2 0 2 7 2 0 2 0 2 0 2 0 2 0 2 0 2 0 2 7 2 0 2 6 2 0 2 7 2 0 2 0 2 0 2 7 2 0 2 6 2 0 2 3 2 0 2 6 2 0 2 7 2 0 2 0 2 0 2 7 2 0 2 6 2 0 2 7 2 0 2 0 2 0 2 0 2 0 2 0 2 0 2 7 2 0 2 0 2 0 2 0] (3)
Original kernel Convolution kernel using our algorithm [START_REF] Aguilar-González | An FPGA 2Dconvolution unit based on the CAPH language[END_REF] In order to detect feature points form an image, we proposed Eq. 4 as corner response metric, where 𝐴, 𝐵, 𝐶 are the convolved matrices and the operator 𝑓{𝑔} is defined as shown in Eq. 5, where 𝐶 𝑛 , 𝐶′ 𝑛 are constant values for a LUT-based square root function [START_REF] Bobda | Distributed Embedded Smart Cameras[END_REF]. In this case, LUT-based functions can be used for hardware implementation for complex operations such as, square roots, Euler function, etc., with low hardware consumption and real-time processing. Finally, we consider that a pixel (𝑖, 𝑗) from an image 𝐼 is a feature point/corner only if 𝑅𝑒𝑠𝑝𝑜𝑛𝑠𝑒(𝑖, 𝑗) satisfy 𝑅𝑒𝑠𝑝𝑜𝑛𝑠𝑒(𝑖, 𝑗) ≥ 𝛼, where 𝛼 is as barrier value provided by the user. For practice, in all our experiments we use a barrier value equal to 25, i.e. 𝛼 = 25.

Feature matching

Considering 𝐴 as a video sequence of 𝑠 frames, we can apply our feature extraction algorithm (Section 3.1), to obtain 𝑔 initial features defined as 𝑥 𝑖 (𝑔) = 𝑥, 𝑦 𝑖 (𝑔) = 𝑦; where 𝑥, 𝑦 are the spatial position for all extracted points. Then, we propose a new feature matching algorithm that searches for a square region centered in any feature 𝑔 in frame 𝑖 that is similar or equal than a similar size square region in frame 𝑖 + 1, located within a search region in frame 𝑖 + 1. In this scenario, we propose Eq. 6 and 7; where 𝑥 𝑖+1 (ℎ), 𝑦 𝑖+1 (ℎ) are the spatial locations for all the features in frame 𝑖 + 1 and, 𝑖 + 1 satisfy 𝑖 + 1 <= 𝑠. 𝐶 measures the similarity between patches and is defined as shown in Eq. 8, where 𝑟 is the search region size, computed via Eq. 9, considering 𝑋, 𝑌 as the horizontal, vertical resolution of the imager. 𝐼 𝑖 and 𝐼 𝑖+1 represent the spatial coordinates of pixels for two different images captured by the imager, and 𝛽, 𝜎 are the spatial location for all the patches in frame i+1, they ranges between -r up to r with increments of 1.

Camera matrix estimation

Given two corresponding point sets: 𝑞 = {(𝑥 1 , 𝑦 1 ), (𝑥 2 , 𝑦 2 ) … (𝑥 𝑛 , 𝑦 𝑛 )} and 𝑔 = {(𝑥 1 , 𝑦 1 ), (𝑥 2 , 𝑦 2 ) … (𝑥 𝑛 , 𝑦 𝑛 )} (these are obtained by applying our feature matching algorithm), a matrix 𝐴 can be estimated as following:

𝐴 [ 𝑞(𝑥 1 ) • 𝑔(𝑥 1 ) 𝑞(𝑥 2 ) • 𝑔(𝑥 2 ) ⋮ 𝑞(𝑥 𝑛 ) • 𝑔(𝑥 𝑛 ) 𝑞(𝑦 1 ) • 𝑔(𝑥 1 ) 𝑞(𝑦 2 ) • 𝑔(𝑥 2 ) ⋮ 𝑞(𝑦 𝑛 ) • 𝑔(𝑥 𝑛 ) 𝑔(𝑥 1 ) 𝑔(𝑥 2 ) ⋮ 𝑔(𝑥 𝑛 ) 𝑞(𝑥 1 ) • 𝑔(𝑦 1 ) 𝑞(𝑥 2 ) • 𝑔(𝑦 2 ) ⋮ 𝑞(𝑥 𝑛 ) • 𝑔(𝑦 𝑛 ) 𝑔(𝑦 1 ) 𝑞(𝑥 1 ) 𝑞(𝑦 1 ) 1 𝑔(𝑦 2 ) 𝑞(𝑥 2 ) 𝑞(𝑦 2 ) 1 ⋮ ⋮ ⋮ ⋮ 𝑔(𝑦 𝑛 ) 𝑞(𝑥 𝑛 ) 𝑞(𝑦 𝑛 ) 1 ],
let [𝑈 𝑆 𝑉] denote the singular value decomposition (SVD) of 𝐴, then, the fundamental matrix 𝐹 can be computed as following:

𝐹 = 𝑈 • 𝑑𝑖𝑎𝑔([𝐷(1,1) 𝐷(2,2)] 0) • 𝑉 ′ .
Then, the essential matrix 𝐸 can be estimated as 𝐸 = 𝐾′𝐹𝐾, where 𝐾 is the calibration matrix for the imager. Finally, by applied singular value decomposition (SVD) over 𝐸 and solving for a close solution, the camera matrix 𝑃 can be estimated. For more details about camera matrix estimation see [START_REF] Hartley | Multiple view geometry in computer vision[END_REF].

Linear triangulation

Given two corresponding point sets: 𝑞 = {(𝑥 1 , 𝑦 1 ), (𝑥 2 , 𝑦 2 ) … (𝑥 𝑛 , 𝑦 𝑛 )}, 𝑔 = {(𝑥 1 , 𝑦 1 ), (𝑥 2 , 𝑦 2 ) … (𝑥 𝑛 , 𝑦 𝑛 )} and considering 𝑃 1 , 𝑃 2 as the camera matrix estimated via the Essential matrix and camera matrix centered at the origin, respectively, we can compute the following variables: 𝐴 1 = 𝑞(𝑥 1 ) • 𝑃 1 (3, 𝑖) -𝑃 1 (1, 𝑖), 𝐴 2 = 𝑞(𝑦 1 ) • 𝑃 1 (3, 𝑖) -𝑃 1 (1, 𝑖), 𝐴 3 = 𝑞(𝑥 1 ) • 𝑃 2 (3, 𝑖) -𝑃 2 (1, 𝑖), 𝐴 3 = 𝑞(𝑦 1 ) • 𝑃 2 (3, 𝑖) -𝑃 2 (1, 𝑖), 𝐴[𝐴 1 𝐴 2 𝐴 3 𝐴 4 ]. Finally, let [𝑈 𝑆 𝑉] denote the singular value decomposition (SVD) over 𝐴, the 3D positions for all the tracked points (the mapping solution of the SLAM process) can be denoted as 𝑉(𝑖, 4). For more details about linear triangulation see [START_REF] Hartley | Multiple view geometry in computer vision[END_REF].

Camera pose estimation

Given two corresponding point sets: 𝑿 = {𝒙 𝟏 , 𝒙 𝟐 … 𝒙 𝒏 }, 𝒑 = {𝒑 𝟏 , 𝒑 𝟐 … 𝒑 𝒏 }, the Least Squares algorithm [START_REF] Hartley | Multiple view geometry in computer vision[END_REF] estimates the translation 𝒕 and rotation 𝑹 that minimizes the sum of the squared re-projection error, as shown in Eq. 10, where 𝒙 𝒊 and 𝒑 𝒊 are feature points matching across two different viewpoints captured by the imager.

𝐸(𝑅, 𝑡) = 1 𝑁 ∑ ∥ 𝑥 𝑖 -𝑅𝑝 𝑖 -𝑡 ∥ 2 𝑁 𝑝 𝑖=1 ( 10
)
The algorithm definition assumes that if the correct correspondences between feature points are known, the correct relative rotation/translation can be calculated in closed form. i.e., considering accurate 2D matching (provided by our feature tracking algorithm), the next step is to compute the center of mass on the input sets 𝑋,𝑝 as shown in Eq. 11 and 12.

𝜇 𝑥 = 1 𝑁 𝑥 ∑ 𝑥 𝑖 𝑁 𝑥 𝑖=1 ( 11
) 𝜇 𝑝 = 1 𝑁 𝑝 ∑ 𝑝 𝑖 𝑁 𝑝 𝑖=1 (12)
Then, subtract the corresponding center of mass from every point in the input sets, as shown in Eq. 13 and 14. denote the singular value decomposition (SVD) of 𝑊, defined as shown in Eq. 15; where 𝑈, 𝑉 are unitary 𝜖ℝ 3×3 , and 𝜎 1 < 𝜎 2 < 𝜎 3 are the singular values of 𝑊. Using camera geometry, it is possible to define 𝑅 = 𝑈𝑉 𝑇 . Thus, If 𝑟𝑎𝑛𝑘(𝑊) = 3, the optimal solution of 𝐸(𝑅, 𝑡) is unique and is given as shown in Eq. 16; where the minimal value of the re-projection function at (𝑅, 𝑡) is computed by Eq. 17. For more details see [START_REF] Hartley | Multiple view geometry in computer vision[END_REF]

𝑊 = 𝑈 [ 𝜎 1 0 0 0 𝜎 2 0 0 0 𝜎 3 ] 𝑉 𝑇 (15) 𝑡 = 𝜇 𝑥 -𝑅𝜇 𝑝 (16) 𝐸(𝑅, 𝑡) = ∑(∥ 𝑥 ′ 𝑖 ∥ 2 +∥ 𝑝 ′ 𝑖 ∥ 2 ) 𝑁 𝑝 𝑖=1 -2(𝜎 1 + 𝜎 2 + 𝜎 3 ) (17) (17)
Note that in a SLAM system, the camera rotation and orientation computed by the Least Squares algorithm represents the localization solution of the SLAM process. In practice, the first camera is located at the origin, and the 𝑅, 𝑡 values represent the second camera localization with respect to the origin (considering feature tracking across frame 1 and 2 from a video sequence captured by the imager). This process must be iterated, i.e., to localize the next camera the Least Squares algorithm requires feature tracking across frames 2 and 3, and then, the third camera localization with respect to the second can be computed.

FPGA ARCHITECTURE

In Fig. 2 an overview of the proposed hardware-software partition is presented. The architecture is centered on a SoC FPGA where all recursive/parallelizable algorithms are accelerated in the FPGA fabric while non-recursive algorithms are implemented in the embedded processor of the SoC FPGA. In this scenario, feature extraction/matching algorithms are implemented in hardware. Algorithms such as: camera pose estimation and 3D reconstruction are non-parallelizable since they require iterative elements and complex mathematics, then a software implementation is more suitable. In general, the basis of the proposed smart camera is the data control unit (see Fig. 2), this unit operates as process-sequencer, where the image subsections are feed to/from the external SDRAM memory using a DMA; as well as the image read out into the circular buffers used to hold local sections of the image and allow for local parallel access and facilitate parallel processing.

Images from the image sensor are stored in an external SDRAM that holds at least 2 frames from the sequence, and later the SDRAM is read by the FPGA to cache parts of the frames into circular buffers. A memory management interface is responsible for data transfers in segments of the image (usually several rows of pixels) to/from the SDRAM. The core of the FPGA architecture are the circular buffers attached to the local processors that can hold temporarily as cache, the image sections from two frames, and that can deliver parallel data to the processors. Two separate image processing accelerators implemented in parallel-pipeline form address the feature extraction/matching step.

Data results from the FPGA processors (the computed feature correspondences between the two frames) is post-processed by the SoC microprocessor to obtain the camera pose and 3D reconstruction for a pair of frames. Finally the SLAM results are sent to a host computer using the Ethernet communication.

The circular buffers

Several image processing applications process pixel data from small neighborhoods from an image, and carrying out a calculation at each position of the neighborhood. For processing purposes, the conventional approach is to store the entire input image into a frame buffer, access the neighborhood's pixels and apply the operation needed to produce the output image. Considering the convolution kernel used in the feature extractor, 25 pixel values are needed to perform the computations each time the kernel is moved and each pixel in the image is read several times. In this case, the memory bandwidth constraints make impossible to obtain all pixels stored in the memory in only one clock cycle, unless any kind of local caching is performed. In [START_REF] Aguilar-González | An FPGA 2Dconvolution unit based on the CAPH language[END_REF] we introduced a circular buffer schema in which input data from the previous 𝑁 rows can be stored using the memory buffers till the moment when the neighborhood is scanned along subsequent rows. To reuse hardware resources when new image data is processed, a shift mechanism between buffer rows is used. Data inside the buffer can be accessed in parallel and each input pixel is fed only once to the FPGA device. In this work, we follow a similar approach to achieve high data reuse and high level of parallelism. Then, feature extraction/matching is carried out in modules where all pixels for the convolution kernel and within the search region can be read in parallel.

The feature extraction accelerator

In Fig. 3 an overview of the proposed FPGA architecture for the feature extraction accelerator is shown. The main objective is to process all data in stream, this approach has proved successful in the past, allowing real-time processing and compact system design [START_REF] Pérez-Patricio | FPGA implementation of an efficient similarity-based adaptive window algorithm for real-time stereo matching[END_REF][START_REF] Di Stefano | A fast area-based stereo matching algorithm[END_REF]. First, the pixels from a frame 𝑖 are stored in a circular buffer that can deliver all pixels centered in 2x2 pixel regions in parallel. Then, the 𝑥, y gradients are computed in parallel. Using the gradients, the 𝐴, 𝐵, 𝐶 matrixes can be computed. Then, the circular buffers can deliver all pixels centered in 5x5 pixel regions in parallel. Using these regions, a 2D spatial convolution can be computed as a stream. In order to achieve high performance for FPGA implementation, we propose the use of reconfigurable convolution units, similar to [START_REF] Aguilar-González | An FPGA 2Dconvolution unit based on the CAPH language[END_REF]. Finally, using the convolved images, the Eq. 4 detects the corresponding feature points. Based on the formulation of the reconfigurable convolution processors [START_REF] Aguilar-González | An FPGA 2Dconvolution unit based on the CAPH language[END_REF], any pixel in the output images have to be defined by integer values. This is highly useful since Eq. 4 is formulated with integer operations, reducing the hardware resources required for the implementation.

The feature matching accelerator

Fig. 4 shows the feature matching accelerator architecture. Similar to the extraction process, this module also delivers all results as a data stream. The module works as follows: first, the pixels from a frame 𝑖 and frame 𝑖 + 1 are stored in a circular buffer that can deliver all pixels centered in size 𝑟2 + 1 × 𝑟2 + 1, where 𝑟 is the search region size. For each feature 𝑞 in frame 𝑖 one square patch of size 𝑟 (which is the size of the search window) centered on the 𝑞 spatial location have to be compared with several square patches in frame 𝑖 + 1. Then, the patch in frame 𝑖 + 1 that minimizes a cost function (Eq. 8) will be the corresponding match with the current location of the feature 𝑞 in frame 𝑖 + 1. For an FPGA implementation, all pixels/patches can be compared in parallel, as illustrated in Fig. 5. Considering that the cost function is the most computational intensive operation, we propose a pixel-parallel/patch-parallel formulation to accelerate the process.

Finally, in order to estimate the current position of a feature 𝑞 (Eq. 6 and 7), a CASE structure allows simple solution of the feature matching problem. i.e., if the cost function response for all 𝛽, 𝜎 values are known, a CASE structure operates as a multiplexer implemented in hardware. This allows to detect the indexes that minimizes the 𝛽, 𝜎 values in simple form (indexes that minimizes the 𝛽, 𝜎 values represent the current position of 𝑞 in frame 𝑖 + 1).

RESULTS

In this section preliminary results are presented for the feature extraction algorithm, the matching algorithm and the SLAM formulation, with a short discussion about the smart camera context.

The feature extraction algorithm

The algorithm was tested with several image sequences and different barrier values. The resulting images are shown in Fig. 6 (detected features are centered in the circles), it demonstrates the flexibility with respect to the corner detection operation. In Tab. 1 performance comparisons with respect to others feature extraction algorithms commonly used in SLAM formulations are shown. In all cases we program the tested algorithm in a Matlab script. Previous algorithm such as, Harris & Stephens [START_REF] Harris | A combined corner and edge detector[END_REF] or Shi-Tomasi [START_REF] Tommasini | Making good features track better[END_REF] require exhaustive and complex mathematical operations, then, they require more processing time that our algorithm. In case of the FAST algorithm [START_REF] Rosten | Fusing points and lines for high performance tracking[END_REF], its main advantage is low processing time. Although it have lowest processing, the mathematical formulation of our algorithm allows efficient FPGA implementation. Thus, the proposed FPGA accelerator have to manage highest processing time and lowest hardware requirements that any FPGA-based accelerator for the FAST algorithm.

feature matching algorithm

In order to present performance comparisons, we implemented several feature matching algorithms suitable for SLAM formulations via Matlab scripts. We matched the features shown in Fig. 6b with the following frame of the video sequence. In Fig. 7, feature matching performance by applying our algorithm is shown. In Tab. 2, we present the mean accuracy for several algorithms previously used in SLAM formulation. In case of the SIFT/ORB algorithms, they allow feature matching along large trajectories in simple form. However, the image degradation between viewpoints introduces data inconsistences that introduce erroneous matches. In order to achieve accuracy required by SLAM applications, statistically robust methods like Random Sample Consensus (RANSAC) have to be implemented to filter erroneous matches. Our algorithm allows high accuracy, superior to SIFT/ORB, suitable for SLAM applications and without any filter or post-processing step. The only limitation compared with SIFT/ORB is that a frame by frame feature matching approach is an exhaustive task. Therefore, the processing time have to be highest than two viewpoint approaches (SIFT/ORB). This can be observed in Tab. 3, were our algorithm and the KLT algorithm, both frame by frame feature-matching algorithms, have highest processing time than SIFT/ORB. Nevertheless, our algorithm has an important advantage because its mathematical formulation allows simple FPGA implementation, suitable for real-time processing. In comparison with previous work we consider that our featurematching approach can provides a convenient framework for SLAM formulations since it enables high accuracy and allows simple FPGA implementation for real-time processing.

The smart camera for monocular SLAM

In order to validate the performance, we tested via Matlab several indoor/outdoor video sequences. Although there are several datasets, in most cases several environmental restrictions such as, controlled illumination, uniform camera movements and less image degradation between frames are considered. Environmental restrictions limit the real-world applications performance since these restrictions often not be present. For our experiments we recorded short video sequences from our local library. In Fig. 8, the 3D map and camera pose obtained with our monocular SLAM approach for two viewpoints from a video sequence (Fig. 7) is presented. The location and matching of feature points were carried out with our formulation. An acceptable 3D point location is obtained, qualitatively similar to what can be obtained with previous state of the art monocular-SLAM algorithms. Therefore, the approach is suitable for satisfactory real-world use. In Tab. 4, numerical results for the test shown in Fig. 8 are presented, similar results were obtained for additional indoor/outdoor video sequences.

CONCLUSIONS

In this article, we introduced new feature extraction and feature matching algorithms suitable for FPGA implementation, complemented with software processing in the context of an SoC FPGA. The proposed feature extraction and matching algorithms were validated with simulation, and a high level FPGA architecture was introduced. The SLAM implementation delivers satisfactory results similar to previous work, and the hardware-software partition will allow for a compact FPGA smart camera. Since many vision algorithms rely on finding and tracking robust features, we consider the work can be extended to environments with several cameras that can collaborate creating complex 3D maps from the environment. Furthermore, the current hardware/software architecture can serve as a framework for additional vision processing integration that can enrich the monocular SLAM process. As work in progress, the actual FPGA implementation is being explored and a smart camera validation will be presented in a future forum.

Figure 1 :

 1 Figure 1: The proposed algorithm

  𝑥 𝑖+1 (ℎ) = ∑ 𝑚𝑖𝑛 𝛽 𝐶(𝛽, 𝜎) ℎ=𝑔 ℎ=1 (6) 𝑦 𝑖+1 (ℎ) = ∑ 𝑚𝑖𝑛 𝜎 𝐶(𝛽, 𝜎) ℎ=𝑔 ℎ=1 (7) 𝐶(𝛽, 𝜎) = ∑ ∑ (𝐼 𝑖 (𝑥 + 𝑢, 𝑦 + 𝑣) -𝐼 𝑖+1 (𝑥 + 𝛽 + 𝑢,

  𝑋 ′ = {𝑥 𝑖 -𝜇 𝑖 } = {𝑥′ 𝑖 } (13) 𝑃 ′ = {𝑝 𝑖 -𝜇 𝑝 } = {𝑝′ 𝑖 } (14)Finally, let 𝑊 = ∑ 𝑥′ 𝑖 𝑝′ 𝑖 𝑇 𝑁 𝑝 𝑖=1

Figure 2 :

 2 Figure 2: An FPGA-based smart camera for monocular-SLAM

Figure 3 :

 3 Figure 3: The FPGA architecture for the feature extraction algorithm

Figure 4 :Figure 5 :

 45 Figure 4: The FPGA architecture for the feature matching algorithm

Figure 6 :

 6 Figure 6: Feature extraction algorithm applied over two consecutive frames from a video sequence.

Figure 7 :

 7 Figure 7: Feature-matching algorithm applied over two consecutive frames form a video sequence.

Figure 7 :

 7 Figure 7: Results for the proposed SLAM algorithm. Left: front view of the 3D reconstruction and the camera pose for 2 frames. Right: top view of the 3D environment showing the cameras and tracked object.

Table 1 . General performance for feature extraction algorithms used in SLAM formulations. Harris [27] Shi-Tomasi [28] FAST [29] proposed

 1

	540 ms	280 ms	120 ms	200 ms

* For this test 16 different images with a 1920x1080 pixel resolution were used

Table 2 . General performance for feature matching algorithms used in SLAM formulations (accuracy) KLT [30] ORB [31] SIFT [32] proposed

 2

	99%	88%	77%	96%

*For this test 16 different images with a 1920x1080 pixel resolution were used

Table 3 . General performance for feature matching algorithms used in SLAM formulations (processing speed) KLT [30] ORB [31] SIFT [32] proposed

 3

	2730 ms	417 ms	839 ms	1480 ms

*For this test 16 different images with a 1920x1080 pixel resolution were used

Table 4 . General performance for our SLAM algorithm 3D density Camera pose error Processing time

 4

	341 features	0.017%	3716 ms

*For this test, two different viewpoints form a video sequence of 1920x1080 pixel resolution were used. Similar results were obtained for 16 additional sequences.