ViTa-SLAM: A Bio-inspired Visuo-Tactile SLAM for Navigation while Interacting with Aliased Environments

RatSLAM is a rat hippocampus-inspired visual Simultaneous Localization and Mapping (SLAM) framework capable of generating semi-metric topological representations of indoor and outdoor environments. Whisker-RatSLAM is a 6D extension of the RatSLAM and primarily focuses on object recognition by generating point clouds of objects based on whisking information. This paper introduces a novel extension to both former works that is referred to as ViTa-SLAM that harnesses both vision and tactile information for performing SLAM. This not only allows the robot to perform natural interaction with the environment whilst navigating, as is normally seen in nature, but also provides a mechanism to fuse non-unique tactile and unique visual data. Compared to the former works, our approach can handle ambiguous scenes in which one sensor alone is not capable of identifying false-positive loop-closures.


I. INTRODUCTION
Robots are often equipped with off-the-shelf sensors like cameras which are use for navigation, however, vision is sensitive to extremes in lighting conditions such as shadows or unpredictable changes in intensity as shown in Fig. 1. Whilst other on-board sensors like laser range finders can be used in such situations they too are impaired by reflective and absorbing surfaces. Situations of sensory paucity also occur in nature, e.g., a rat moving through a maze in ill-lit conditions as illustrated in Fig. 2, however, evolutionary processes has equipped animals to gracefully accommodate such scenarios. Given the coarse vision and visually aliased narrow tunnels through which a rat often needs to navigate, they are known to rely on tactile feedback derived from whiskers aside from vision to decipher their own location [1]. Conventional robots lack such a robust capability to interact with their environment through contact. Thus, biomimetic robots are gaining traction [2] which has now made it possible to harness visual and tactile sensory modalities for informed decision making. However, it still remains unclear how to best process and combine information from disparate sensory modalities to aid in spatial navigation.
Previous works on visuo-tactile sensor fusion, such as [3], address the challenge of navigating in unstructured scenes whilst interacting with objects. For this, a low cost camera is mounted such that its field-of-view overlaps with that of a deformable foam rod to acquire coherent data from two sensory modalities.  Alt et al., then demonstrated the application of such systems in household settings [4]. Bhattacharjee et al., tackle the problem of generating a dense tactile map of the environment in which a humanoid robot is operating [5]. For this, a tactile sleeve is utilized along with a complementary Kinect RGB-D camera with redundant field-of-view. In [6], a biologically-inspired visual scene representation inference mechanism is devised that allows a robot to perform haptic interactions with the objects. Like this, the humanoid robot (primary test platform for this work) can poke, push, grasp and visually observe the objects just like an infant. It was argued that this mechanism gets rid of the need to learn visual scene representations by deploying deep-learning methods to train on passive observations and also highlights the need for environmental interactions which is one of the primary themes of this work. Robotic grasping is a challenging task with researchers typically focusing on purely vision, which provides coarse-grained information about object location, shape, and size, or purely on haptic-feedback which provides complementary fine-grained cues about contact forces, textures, local shape around contact points, and deformability, all of which are critical for evaluating an ongoing grasp. In [7], the authors proposed harnessing both visual and tactile data in an end-to-end framework to evaluate the current grasp and maintain estimates of the required grasp adjustments in the future.
Most of the aforementioned works have a requirement for a redundant field-of-view to obtain coherent information from sensory modalities of varying sensing range. Additionally, these methods are mostly concerned with developing a dense haptic object/scene map using a tactile sensor with complementary vision sensor. Whilst these methods allow for environmental interactions they are primarily designed for tactile object exploration and not for performing spatial navigation using mobile robot platforms know as Simultaneous Localization and Mapping (SLAM). To this end, our preliminary results were presented in [8] which provided promising results when jointly considering visuo-tactile sensory modalities for addressing the SLAM problem. Subsequently, a robust sensor fusion algorithm has been developed to integrate information from unique and non-unique sensory modalities such as cameras and whiskers, respectively. Additionally, performance metrics are presented herewith to compare and evaluate model performance against vision or tactile only sensing.

II. BIO-INSPIRED SLAM
This work draws inspiration from two well-known bioinspired SLAM frameworks: RatSLAM, a rat hippocampal model based visual SLAM architecture [9]; and Whisker-RatSLAM, an extension of RatSLAM aimed primarily at tactile object exploration [10]. This work relies on slightly modified variants of these models that are summarized here, followed by a description of the proposed ViTa-SLAM.
For each of the models, an overall system architecture is provided using the following convention: nodes represented by right isosceles triangles represent raw sensory data; nodes represented by ellipse(s) represent pre-processing of sensory data before they are converted to input features represented by rounded boxes. The outputs from the models are represented by regular boxes, the pre-processing and feature generation stages are highlighted in light blue and light red blocks, respectively.

A. Visual-SLAM
When investigating the way rodents navigate from a bioinspired perspective, RatSLAM as introduced in [9], [11]- [14], has been proven to be a capable visual SLAM method. RatSLAM is loosely based on the neural processes underlying navigation in the rodent (primarily rat) brain, more specifically the hippocampus. Fig. 3 shows an overview of the visual-SLAM implementation used in this work.
During the preprocessing phase, the input of a camera (visual data) is downsampled to reduce computational cost and to simulate the coarse vision of rats. In this process the incoming visual data is cropped to remove areas that do not provide unique features, like for example the ground. The cropped image is subsampled and converted to greyscale as shown in Fig. 4.
The preprocessed sensory information is now parsed through 3 major components of the RatSLAM architecture:  The pose cells [9] encode the robot's current best pose estimate. Pose cells are represented by a Continuous Attractor Network (CAN) [11,Ch. 4], the posecell network, to resemble the grid cells as introduced in [15]. The grid cells are neurons found in many mammals and are shown to be used in navigation. In the 3D posecell network, the robot's pose estimate (x, y position and heading angle γ) is encoded as a energy packet that is moved through energy injection based on odometry and place recognition.

Preprocessing Templates
The local view (LV) cells are an expandable array of units used to store the distinct visual scenes as a visual template in the environment using a low resolution subsampled frame of pixels. The visual template generated from the current view is compared to all existing view templates by shifting them relative to each other. If the current view is novel, a local view cell is linked with the centroid of the dominant activity package in the pose cells at the time when a scene is observed. When a scene is seen again, the local view cell injects activity into the pose cells.
The experience map is a semi-metric topological representation of the robot's path in the environment generated by combining information from the pose cells and local view cells into experiences. Each experience is related to the pose cell and local view cell networks via the following 4-tuple: < x, y, γ, V > where x, y, γ represent the location of the cell in the PC network while V corresponds to the view associated with the LV cell that relates to the queried experience [16].
Initially the robot relies on odometry which is subject to an accumulating error. When loop closure events happen, meaning a scene has been seen already, the pose estimate based on the odometry is compared to the pose of the experience and graph relaxation is applied [16].
The following differences to RatSLAM have been introduced in the visual-SLAM implementation. First, we use odometry from the robot instead of visual odometry as was originally done to determine the translational and rotational speeds of the robot. Second, the method of template matching and generation has been modified to account for multiple sensory modalities. Third, the posecell network (PC) is now capable of handling a wider range of robot motion such as moving sideways.

B. Tactile-SLAM
Whisker-RatSLAM is a 6D tactile SLAM algorithm inspired by RatSLAM. Instead of input from a camera, it uses a tactile whisker-array mounted on a robot as its only sensor [17], [18]. The whisker-array consists of 6 × 4 whiskers, each capable of measuring the point of whisker contact in 3D space, and the 2D deflection force at their base [19]. Whisker-RatSLAM [10] has been demonstrated mapping objects and localizing the whisker array relative to the surface of an object. Similar to RatSLAM, Whisker-RatSLAM generates a semi-metric topological map, the object exploration map, which contains complex 6DOF experience nodes.
In [10], the authors proposed combining these object exploration maps with simple 3DOF experience map generated using RatSLAM with the whisker-input resulting in a topological terrain exploration map with two different types of experience nodes. Fig. 5 shows an overview of the Whisker-RatSLAM algorithm. The tactile data acquired by whisking encompasses 3D contact point cloud of the object (3D Cts.) and the deflection data (Defl.). The point cloud is used to generate the Point Feature Histogram (PFH) while the deflection data is used to generate Slope Distribution Array (SDA). Both PFH and SDA are then fused to obtain a 6D Feature Cell (FC). Similar to the RatSLAM experience map, the pose grid cells and FC that were active at a specific 6D pose of the whisker-array are associated with each other and combined into experience nodes. The experience in this case is defined as the 7-tuple: < x, y, z, α, β, γ, F > where x, y, z, α, β, γ represents the 6D pose including euler angles for orientation and F ← {P F H ∪ SDA} represents the features associated with that experience. The experience node form the object exploration map (Obj. Expl. Map). In order to adapt the activation of the pose cell in accord with the robot motion, the odometry information is also used in the pose grid.
The tactile-SLAM implementation is based on Whisker-RatSLAM, but instead of a 6D posecell network this work uses the same 3D posecell network as the visual-SLAM implementation to allow compatibility and to reduce computation cost for navigation in 3D space. Furthermore, the tactile-SLAM implementation used in this work does not use feature cells, but instead combines the SDA and PFH data into 3D tactile template that are used in a similar way as 3D visual templates. Fig. 5 shows an overview of the tactile-SLAM algorithm. The tactile data acquired by whisking encompasses 3D contact point cloud of the object (3D Cts.) and the deflection data (Defl.). The point cloud is used to generate the Point Feature Histogram (PFH) while the deflection data is used to generate Slope Distribution Array (SDA). Both PFH and SDA are then fused to obtain a tactile template (T). Similar to the RatSLAM experience map, the pose grid cells and T that were active at a specific pose of the whisker-array, are associated with each other and combined into experience nodes. The experiences are, opposed to the 7-tuple used in Whisker-RatSLAM, defined as a 4-tuple: < x, y, γ, T > and T ← {P F H ∪SDA} represents the tactile template associated with that experience. The experience nodes also form a semi-metric experience map similar to the visual-SLAM method. Similar to the visual-SLAM method, the robot's odometry information is also used to move the pose grid.
To generate tactile information using whiskers, one challenge is how to control the whisker-array in order to improve the quality of the sensory information. Previous research on rats [20] has identified a number of whisking strategies that rodents use to potentially improve the sensory information they obtain. One of these strategies is called Rapid Cessation of Protraction (RCP) and refers to the rapid reduction in motor drive applied to the whisker when it makes contact with an object during the protraction phase of exploratory whisking [21]. This effectively reduces the magnitude of bend of the whisker upon contact which in artificial arrays, such as shown in [22], improves the quality of sensory information by constraining the range of sensory response to a region best suited for signal processing. Furthermore, damage to the whiskers from contact is significantly reduced.

C. ViTa-SLAM
In this section, we present the details of our novel visuotactile SLAM algorithm which we refer to as ViTa-SLAM.
The overall system architecture for ViTa-SLAM is shown in Fig. 6: 3 kinds of raw sensory data: tactile, visual, and odometry are now utilized simultaneously. Tactile and visual data are converted into visuo-tactile templates (T, V), respectively and hence, need to be pre-processed. A 3D pose cell network is maintained. The experience in this approach is now defined as a 5-tuple: < x, y, γ, V, T > where V is a visual template and T is a tactile template at the 3D pose given by x, y and γ.
The experience map in this case will be referred to as vita map. In contrast with the conventional experience map, the vita map's nodes contains visual and tactile data. The nodes are termed sparse node if the tactile data is empty and dense node otherwise. As an example, when the whiskers do not make contact, the whisker tactile information is not providing any information while the camera can still acquire novel scene information. When the whiskers are whisking a wall/landmark, both the camera and whiskers yield features that allow the creation of informative dense nodes which greatly help visuotactile SLAM.  Algorithm 1 describes ViTa-SLAM in more detail. The visual and tactile processes are running continuously in parallel to the ViTa-SLAM node, as shown by the -symbol and store the current visual and tactile templates V cur and T cur . The visual process follows the same steps as shown in Fig. 4. The resulting visual template is called V cur .
Similarly, the Tactile process is pre-processing the input data consisting of the xy-deflection angles and the whisker contact points in world frame. In line 2, the point feature histogram (P F H) is generated by creating a N -dimensional histogram of the contact points with B bins per dimension. The resulting histogram is then flattened into a N B histogram. For each whisker, the slope of the xy-deflection between the initial contact to the maximum contact during one whisk cycle 1 is computed. The result is the slope distribution array (SDA). The current tactile template containing P F H and SDA is saved as T cur . The two components can be extracted as [t] P F H and [t] SDA .
In the ViTa-SLAM process, if a whisk cycle has been completed, the data stored in the visual and tactile processes, V cur and T cur , is extracted. In line 4, V cur and T cur are matched against all old visual and tactile templates and the id with the closest match m and the corresponding error are returned. Finally, the error is used to determine if a novel template has been detected or a match with an old template has occurred. In either case, energy is injected at the template with match id m. img ←crop image(img) 3: img ←subsample(img) 4: img ← to greyscale(img) if ≤ τ then 6: inject(match id) , m ← arg min( ) 10: return , m After this process, the current visual and tactile templates are appended to the memory and the template match ID is published for experience map generation. The template matching function computes by comparing the current template to all visual and tactile templates in the memory. The visual error V err is computed as the pairwise sum of absolute differences between V cur and all visual templates in the memory. For the tactile data similarly, the PFH and SDA are treated separately and the respective errors (P F H err and SDA err ) are computed. A weighted sum of all obtained error terms yields the error cur between the current visuo-tactile templates and the ones in the memory as: where, In Eq. (1), |·| L1 represents the L1 norm between the corresponding terms. α, β and γ are scaling factors for the respective errors which represent the standard deviations of the raw sensory data. Finally, the returned match ID (m), is the ID of the combined template with the lowest .

III. EXPERIMENTAL SETUP
In this section, we describe the operational environment and the robot platform that were used for empirical validation of the proposed ViTa-SLAM algorithm.

A. Robot Platform
The robot platform used for this research is called WhiskEye (Fig. 7a) the design of which is based on a previous whiskered robot [18]. WhiskEye is composed of a Festo Robotino body, a 3 DoF neck, and a 3D printed head. Mounted on the head are two monocular cameras and an artificial whisker array consisting of 24 macro-vibrissae whiskers arranged into 4 rows of 6 whiskers distributed around a micro-vibrissae array as the nose. Each macro-vibrissae is instrumented to detect 2D deflections of the whisker shaft measured at its base, and is actuated using a small BLDC motor to reproduce the active whisking behaviour observed in small mammals. Motor commands to each whisker, each DoF in the neck, and to the Robotino body of WhiskEye are issued from a small form factor PC mounted on the body running the ROS execution framework. This allows for candidate control architectures to be developed and deployed using the ROS interface to the physical platform or the Gazebo simulation (shown in Fig. 7b) of WhiskEye as used in this study.

B. Operational Environment
As a proof of concept, the algorithm was primarily tested in simulated aliased environments. For this, a 6 × 6 m 2 arena with 4 wall-mounted visual and 3 tactile landmarks was designed as shown in Fig. 8. Within this environment, the robot was made to revolve around the center of the arena, whilst facing outwards, with a radius of 1 m.

IV. PERFORMANCE METRICS
The following metrics were used to evaluate the performance of the proposed ViTa-SLAM against the Visual-SLAM and Tactile-SLAM.

1) Localization Error Metric (LEM)
The localization error metric (LEM) measures the root mean squared error (RMSE) between the true pose and the estimated pose where the error is calculated separately for position and orientation. Thus, where, In Eq. (2),(·) refers to estimated position (orientation) while (·) refers to true position (orientation).

2) Experience Metric (ExM)
The Experience metric (ExM) introduced in [22] provides a performance measure for algorithms like RatSLAM that produce semi-metric topological maps with loop closures. The ExM is comprised of the average rate of re-localization (ARR) and the average rate of correct re-localization (ARCR). The ARR is defined as the ratio of re-localizations to total number of experiences excluding the base set. The base set is a set of initial experiences that has to be selected to define the main loop closure and to provide a reference for future experience to relocalize with The ARCR is defined as the ratio of correct re-localizations to the total number of re-localizations. High values (close to 1) for both factors indicate high certainty in the pose estimate. In order to determine if a re-localization is correct or incorrect, a threshold is used to compare the accuracy of the estimated pose to the ground truth pose. An experience following an incorrect re-localization is labeled invalid until a correct re-localization occurs. An experience following a correct re-localization is labeled valid.

3) Energy Metric (EnM)
In [25], energy metric was proposed as a generic metric for evalution of variety of SLAM algorithms. The SLAM performance was measured in terms of the energy required to transform the SLAM trajectory to the true trajectory. Let N represent the number of relations between experiences in vita map and their corresponding sample points from the set of collected pose data. Then, δ i,j = x i x j represents the transformation from node x i to x j . If T (·) and R(·) represent the translation and rotation operations, then the energy metric (EnM) can be defined as:

V. PERFORMANCE EVALUATION
In this section, we compare the performance of ViTa-SLAM against Visual and Tactile SLAM approaches using the metrics   described above. The experience maps hence obtained are shown in Fig. 9 while the empirical summary of all the metrics is given in Table I. 2 The Energy Metric (EnM) shows that ViTa-SLAM requires less energy to to transform the trajectory to the ground truth by an order of magnitude. This confirms what can be seen from Fig. 9, the experience maps of visual and tactile only SLAM are highly skewed as the result of wrong loop closures.
To further evaluate the quality of loop closure detection we use the Experience Metric (ExM) with the thresholds for position and angular accuracy set to 0.08 m and 4.6 • with the base set defined as the experiences generated during the first full rotation. The ARR and ARCR for ViTa-SLAM show that ViTa-SLAM is able to re-localize more often and correctly in all cases while the other methods always fail. The reason for this failure becomes evident in Fig. 10a and 10b. For the visual-and tactile-SLAM methods, false positive re-localizations already occur in the base set as a result of the aliased environment. As opposed to this, ViTa-SLAM successfully completes one rotation and correctly closes the loop.
The LEM further confirms these findings as the mean pose estimation accuracy of ViTa-SLAM is clearly lower than the state-of-the-art methods.

VI. CONCLUSION AND FUTURE WORKS
This work demonstrated a novel bio-inspired multi-sensory SLAM mechanism for a robot exploring and interacting with 2 Video demonstration available here an environment that presents ambiguous cues. While previous attempts had been made to propose bio-inspired multi-sensory fusion, no prior research allowed for either environmental interactions through contact or fusion of unique and nonunique sensory information. To this end, ViTa-SLAM was presented which utilizes long-range visual and short-range whisker (tactile) sensory information for efficient bio-inspired SLAM. When comparing against earlier approaches that use only vision like RatSLAM or only tactile information like the Whisker-RatSLAM, it was shown that visuo-tactile sensor fusion can handle ambiguities that would otherwise lead to false positive loop-closure detection.
In the future, we plan to investigate bio-inspired sensor fusion approaches based on predictive coding. We also plan to improve the robustness and acuity of the whisking behaviour whilst incorporating spatial attention mechanisms as is seen in rats [26]. Additionally, active spatial exploration strategies will be explored to improve the accuracy of localization and speed of mapping.