FLAF: Focal Line and Feature-constrained Active View Planning for Visual Teach and Repeat
Changfei Fu Weinan Chen Wenjun Xu Hong Zhang









FLAF: Focal Line and Feature-constrained Active View Planning for Visual Teach and Repeat

*Corresponding author (hzhang@sustech.edu.cn)

Changfei Fu and Hong Zhang are with the Shenzhen Key
Laboratory of Robotics and Computer Vision, Southern University of
Science and Technology (SUSTech), and the Department of Electrical and
Electronic Engineering, SUSTech, Shenzhen, China. Changfei Fu and Wenjun Xu are also with the Peng Cheng National Laboratory, Shenzhen, China. Weinan Chen is with the Biomimetic and Intelligent Robotics Lab, Guangdong University of Technology, Guangzhou, China. This work was supported by the Shenzhen Key Laboratory of Robotics and Computer Vision (ZDSYS20220330160557001).

    
 Changfei Fu, Weinan Chen, Wenjun Xu, and Hong Zhang*

    Received Month dd, yyyy; accepted Month dd, yyyy
================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================



§ ABSTRACT 
This paper presents FLAF, a focal line and feature-constrained active view planning method for tracking failure avoidance in feature-based visual navigation of mobile robots. Our FLAF-based visual navigation is built upon a feature-based visual teach and repeat (VT&R) framework, which supports many robotic applications by teaching a robot to navigate on various paths that cover a significant portion of daily autonomous navigation requirements. However, tracking failure in feature-based visual simultaneous localization and mapping (VSLAM) caused by textureless regions in human-made environments is still limiting VT&R to be adopted in the real world. To address this problem, the proposed view planner is integrated into a feature-based visual SLAM system to build up an active VT&R system that avoids tracking failure. In our system, a pan-tilt unit (PTU)-based active camera is mounted on the mobile robot. Using FLAF, the active camera-based VSLAM operates during the teaching phase to construct a complete path map and in the repeat phase to maintain stable localization. FLAF orients the robot toward more map points to avoid mapping failures during path learning and toward more feature-identifiable map points beneficial for localization while following the learned trajectory. Experiments in real scenarios demonstrate that FLAF outperforms the methods that do not consider feature-identifiability, and our active VT&R system performs well in complex environments by effectively dealing with low-texture regions.



VT&R, Active View Planning, Low Texture




§ INTRODUCTION

Learning to cruise a path while traversing it is a fundamental capability for mobile robots<cit.>. Considering that humans and vehicles mainly rely on various flexibly fixed paths to repeatedly shuttle between multiple locations, Teach and Repeat (T&R)<cit.> is an essential technique for robots to learn to navigate the paths that cover a major part of autonomous navigation requirements. This technique can support many robotic applications, such as household robots traveling between different rooms<cit.>, delivery robots taking goods from the logistics center to the target building<cit.>, and autonomous buses following a mostly fixed trajectory.

As a type of natural visual sensor, monocular cameras are cost-effective, energy-efficient, and versatile, making them suitable for a wide range of environments and applications. The T&R approaches that predominantly utilize visual sensors are referred to as Visual Teach and Repeat (VT&R)<cit.>, which is a significant motivation of the research in visual simultaneous localization and mapping (VSLAM)<cit.>. Our active VT&R system integrates a pan-tilt unit (PTU)-based active camera with feature-based VSLAM (see Fig. <ref>). During the teaching phase, the mobile robot reconstructs the surrounding landmarks while traversing a path under guidance<cit.>. In the repeat phase, the previously saved path map is reloaded to localize the robot for navigating the taught trajectory.






Although feature-based VSLAM systems<cit.> achieve impressive robustness and stability in large indoor and outdoor environments<cit.>, they all suffer from the view angle-dependent affine change<cit.> of features, and the map for real-time localization are usually too sparse for other complex applications<cit.>. Fortunately, the limited view-angle invariance of features and sparse map for reliable localization is enough for path following in VT&R. In this work, we achieved a high success rate and reliability in passive VT&R (using a fixed camera) with feature-based monocular VSLAM. However, textureless regions in human-made environments are still limiting VSLAM and VT&R to be used in the real world. To solve this problem, existing active SLAM works mostly choose to change the robot trajectory, which is not suitable for VT&R<cit.>. In order to use an active camera to actively select informative views without interfering in the T&R trajectory, our work focuses on designing an active view planning method with active camera-based VSLAM suitable for VT&R.

Our motivation is to design a feature-based VT&R system coupled with active view planning for tracking failure avoidance. Previous active camera-based VSLAM systems<cit.> have primarily focused on maintaining stable localization during the mapping process but have not demonstrated how to effectively reuse the map with active view planning for navigation tasks. The key difference between mapping and reusing the map is that there are more choices of map points when the map is complete during navigation, compared to the mapping process when the active camera has no choice but to orient to the newly built map points. As shown in Fig. <ref>, existing methods<cit.> that rotate the active camera to fixate on more map points that are beneficial for tracking may fail in some VT&R cases because they do not consider the feature identifiability<cit.>, which refers to the ability of the feature detector to identify a 3D map point. While the robot is moving forward in the direction indicated by the arrows (see Fig. 2), existing view planning methods mostly focus the active camera on the regions dense with map points. In many situations, these map points targeted by the active camera are triangulated by earlier keyframes taken from viewpoints significantly different from the current one, making them unidentifiable by the feature algorithms due to their substantially different visual appearances. To address this, we have designed an active camera-based VT&R system featuring an innovative active view planning method that accounts for the view angles of map points. Our FLAF-based active view planning is focal line-centric, as its direction dictates the angles of the active camera.


In this paper we present a visual navigation system with the PTU and a novel active view planning method for feature-based VT&R. The contributions of our work are summarized as follows: (1) We combine VSLAM, motion control in repeat, and feature-based active view planning to overcome the low-texture problem. To the best of our knowledge, this is the first demonstration of VT&R system coupled with active view planning. (2) In real-scene experiments of VT&R, we find the active view planner should be redesigned in the repeat phase because of the unidentifiable points in the already existing complete map. We identify that the focal line vector is key to evaluating the angles of a rotatable camera. (3) We propose an online focal line and feature (FLAF)-constrained active view planning method that rewards the camera orientations that observe more feature identifiable map points at their mean-view directions and is suitable for both phases of VT&R.




§ RELATED WORK

Our active VT&R is based on VSLAM system tightly coupled with active view planning for tracking failure avoidance.



 §.§ Visual Teach and Repeat


The classical work of VT&R<cit.> using a feature-based stereo VSLAM was later developed as VT&R2<cit.>, which utilizes multiple taught experiences to address environmental appearance change. Despite the significant progress, this series of works all suffer from the tracking failure caused by low-texture regions<cit.> as they all rely on fixed cameras and feature-based VSLAM. Based on the well-established VT&R2<cit.>, Warren et al.<cit.> build a gimbal-stabilized VT&R system in which the gimbal is passively utilized to stabilize the camera or manually steered to avoid degeneracy in the teach phase. In the repeat phase, the camera actively rotates to the nearest keyframe in the taught graph. Although this work justified the necessity of an active camera in VT&R, the gimbal is manually steered in the teach phase instead of autonomous operation. Previous works designed various active view planning methods<cit.> for active camera-based VSLAM. However, the recently proposed uncertainty-driven view planning (UDVP)<cit.> that rotates the camera toward more nearby map points easily fails in the repeat phase for it ignores the affine change of features<cit.> (Fig. <ref>). To solve these problems, we implement the same autonomous view planning method (FLAF) in both phases of VT&R.





Faced with the aforementioned tracking failure and occlusion, Mattamala et al. <cit.> designed a VT&R system allowing the quadruped to switch between multiple cameras set on different positions of the robot. However, each one among the cameras faces the same problem of tracking failure. Every camera builds several sub-maps where completion and consistency are difficult to guarantee in sub-map merging without well dealing with the challenging views. Our proposed active VSLAM can help each one of the multiple cameras in<cit.> to improve the performance of its system. In the meanwhile, our VT&R system is more natural and compact to use fewer computing resources.








 §.§ Visual Simultaneous Localization and Mapping


Our VT&R system consists of a 3D reconstruction module for the mobile robot to remember a traversed path. In the teach phase, the robot learns the landmarks in feature-based VSLAM. With the input of an image set that shares observations of the environment, the task of estimating camera motions and a geometrical reconstruction is called Structure From Motion (SfM)<cit.>. For a mobile robot with a moving video camera, a system that performs SfM for every image as it is captured is called real-time SfM or VSLAM. Specifically, our proposed active view planning method is designed according to the local map built by VSLAM systems<cit.>.

The most popular implementations of VSLAM<cit.> follow the principle of aligning the current image to the already-built map for camera localization. The data association between current frame and keyframes and bundle adjustment (BA) are conducted locally and globally to obtain a consistent map<cit.>. To use time-consuming bundle adjustment in SLAM for optimizing a consistent map, a hierarchical optimization strategy is proposed with the concept of window and local bundle adjustment<cit.>. 

In <cit.>, a sophisticated local map is designed to align the current frame and implement the local BA. Motion estimation by aligning the current image to the local map is called local map tracking<cit.>. Deng<cit.> et al. identify a particular tracking failure caused by the incapability of associating enough features. In <cit.> the authors also indicate that the possibility of tracking failure approximates zero if the associated map points is more than a level.




 §.§ Active View Planning for VSLAM


Seminal work of active view planning for VSLAM is <cit.>, in which Davison et al. indicates the solution of active vision in navigation is using serial views on a succession of features for stable localization and deciding the time to explore new features. Following this idea, <cit.> proposes to observe the unexplored regions and reset the active camera if features are not enough for localization. However, this strategy doesn't guarantee an accurate localization because the view direction same to the initial one doesn't capture the same initial image as the robot is moving. 

In active camera-based VSLAM, the primary problem before the exploration of new features is trying its best to maintain a stable and accurate localization<cit.>. In <cit.> the authors propose the UDVP method to measure the quality of the camera angles with respect to every map point. This UDVP model shows fine performance in capturing more existing map points in the Field of View (FoV) of the camera. However, it (shown in Fig. <ref>) doesn't consider the feature identifiability in the repeat phase which results in tracking failure by looking at unrecognizable map points. Our FLAF method uses the observation model shown in Fig. <ref>, which utilizes the angle between the camera focal line and the line of sight to a map point to make the camera capture more built map points. In the meanwhile, the angle between the imaging light path of the point and the normal line of the map point is measured to qualify the feature identifiability. 



The idea to consider both the angles of α_1 and α_2 shown in Fig. 3 was first described in the VSLAM system of<cit.> for feature selection, which serves as the foundation for our system. This strategy is also utilized in the active SLAM method presented in <cit.>, which employs a fixed camera for both navigation and exploration. The model in <cit.> defines specific ranges of distance and α_2 to screen map points within the camera's frustum. The "FLAF without scoring" method in our comparison experiments can be seen as an implementation of active VT&R using the model from <cit.>, despite the differences in the robot and task. It is noteworthy that Mostegel et al.<cit.> identified several metrics for feature recognition and validated the effectiveness of using a cosine function to measure the feature recognition probability, which indicates the likelihood of feature identification from different view angles.





§ APPROACH


We build our VT&R system on an active camera-based VSLAM framework shown in Fig. <ref>, which integrates ORB-SLAM2<cit.> and our FLAF-constrained active view planning. Our method for active camera-based path following is similar to that described in <cit.>. During the construction of this navigation system, we observed frequent failures in the repeat phase when feature identifiability was not considered. The core idea of our formulation is to rotate the camera toward optimal directions relative to the map points, thereby improving the performance of the feature detector by taking into consideration the feature identifiability of these map points.

To increase the number of map points in the central area of the field of view, we designed a scoring function based on cos(α_1) to rotate the camera toward regions dense with map points. Each map point within the FoV contributes to the total of this scoring function. Map points with smaller α_1 values receive higher scores, which encourages the clustering of map points in the central area of the field of view. Consequently, camera poses that capture more map points with smaller α_1 in their FoV achieve higher scores. This strategy is similar to the UDVP model<cit.>, which uses a sigmoid function with higher computation complexity.

Another goal of our design is to score the map points with good feature identifiability relative to the current camera pose. Feature identifiability refers to the ability of the feature detector to recognize the map point from a specific position and orientation. With the path map built, the VT&R system can determine the existence of a map point. But if the robot rotates the camera toward this map point from an arbitrary position, there is a probability<cit.> that this map point may not be detected in the current image due to affine changes<cit.>. In the teach phase, only those map points observed and associated with several different keyframes are retained in the map. We assume the keyframes that observe the same map point define a view distribution within which this map point can be identified. The mean viewing direction is considered the normal of the map point, around which the view distribution is defined. This definition of the map point normal was also used in <cit.>. Regarding the treatment of the normal of a feature, Mostegel et al.<cit.> justified using cosα_2 as the metric of the probability of feature identification to account for the observation of a feature from different viewpoints and view angles. Our scoring function, cos(α_1)·cos(α_2), multiplies these two cosine functions to prioritize map points that score highly on both metrics. 



 §.§ VSLAM with Active Camera

While the mobile robot is moving on a path, images with equally spaced timestamps are sequentially captured by the moving camera and sent to the VSLAM system. For every input image 𝐈_k:ℝ^2→ℝ, ORB features<cit.> are extracted and sent to the local-map tracking module, in which features are aligned with the local map to estimate current camera pose 𝐗_k,w∈SE(3) in world coordinate. This process of computing a camera pose according to the local map is called local map tracking. If enough new features are found, this frame is decided as a keyframe and these features are triangulated into the map based on data association between local keyframes. This process of projecting new keyframes and features into the map space (world coordinate) is called local mapping. 


A local map including a network of keframes {F_i} connected according to feature matching and thier associated map points {𝐏_j^i|𝐏_j^i∈ℝ^3,j=0,1,2,...,m_i} are denoted with:

    M_l={𝐏_0^i,𝐏_1^i,...,𝐏_m_i^i,F_i|i=0,1,2,...,n_i}

where m_i is the quantity of the map points associated with the i-th keyframe and n_i is the quantity of the keyframes in the local map. A map point only refers to a 3D coordinate in map space, but its descriptor can be computed from the keyframe by which the point is triangulated. According to the descriptor matching, keyframes share observations with 𝐈 to make up the local map with their associated map points. 

For any map point 𝐏_j^i in M_l visible frustum of current camera pose can be matched with an ORB feature at 𝐩_j^i∈ℝ^2 on 𝐈_k, we reproject it onto the 𝐈_k by pinhole camera model π:ℝ^3→ℝ^2 and get the reprojected coordinate π(𝐏_j^i)∈ℝ^2. Because of the mapping and localization uncertainty, there exists an error 𝐞_i,j∈ℝ^2 of the j-th map point in i-th keyframe:

    𝐞_i,j=𝐩_j^i-π(𝐏_j^i)

But not all points of M_l are within visible frustum of the camera and can be matched with an ORB feature in 𝐈_k. We collect any map point 𝐏_j^i in M_l which can be identified by current view as a set S={𝐏_j^i|𝐏_j^i can be identified by 𝐈_k}. Then we conduct the local map tracking by minimizing the cost function<cit.>:

    𝐗_k^*=𝐗_kmini,j,
    𝐏_j^i∈ S∑𝐞_i,j^𝖳Ω_i,j^-1𝐞_i,j

where 𝐗_k^*∈SE(3) represents the pose estimation of 𝐈_k. The magnitude of map points within the visible frustum of the camera that can be matched with an ORB feature in 𝐈_k is represented as:

    N_S=f(𝐗_k,M_l)

to indicate that N_S is a function of current camera pose and the local map.

According to the results in <cit.>, the pose estimation shown by Equation (<ref>) fails if the magnitude of S is below a threshold. Therefore, <cit.> proposes a view planner to increase N_S. To achieve this idea by our sampling-based optimization, the pan-tilt angles of the PTU are sampled as 𝐪=(pan,tilt) and transformed into 𝐓_pt(𝐪)∈SE(3) to rotate the camera to 𝐗_k', which is directly evaluated:

    𝐗_k' = 𝐓_pt(𝐪) ∗𝐗_k





 §.§ Active View Planning for Feature-based VT&R

In the classical feature-based VSLAM<cit.>, on which we build our active camera-based SLAM system, the next frame is typically decided by the trajectory of the robot. To avoid low-texture views, we make passive VSLAM tightly coupled with a feature-based active view planning module. Specifically, we insert (Fig. <ref>) the active view planning module between the local map tracking and the local mapping because they are separated into different threads in implementation. After local map tracking, we have the local map and current camera pose needed in active view planning. Based on the local map and current camera pose, the next best angles of the PTU can be decided. Thus we achieve using current partial perception results as feedback to control the sensor and image input. This process reflects the concept called “active vision” in the robotics community.

Using the UDVP observation model<cit.>, we control the active camera to see more and closer map points. Given a map point in the local map, the UDVP<cit.> model awards the PTU angles with a smaller distance to it and a smaller view angle between the line of sight to it and the camera's focal line. In the teach phase, the active camera always looks at the just-built partial map. In this case, it's easy to extract ORB features and match them with those in past keyframes because the just-built map points conform to above criteria of UDVP model. Thus N_S=f(𝐗_w) is increased and tracking failure probability is decreased compared to passive VSLAM. This effect can be represented by the inequality:

    f(𝐓_pt𝐗_k) > f(𝐗_k)


However, in the repeat phase, a complete map of the taught path is built. Although there exist more local map points that are close to the optical center and have a small angle between sight line OP and focal line n_c (α_1 shown in Fig. <ref>), some of the points are not identified by the feature matcher because of a big angle between the focal line and mean view angle of the map point. This situation results in the higher failure rate of repeat. This problem can be summarized as “looking at points visible but not identifiable makes the tracking fail”. We draw Fig. <ref> to explain the particular VT&R failure.

To solve this problem, we use the observation model proposed in <cit.> shown in Fig. <ref>. One distance and two angles are measured for every pan-tilt sample and map point to support our design principles:


    
  * Scoring the pan-tilt sample that makes the map point in the range of (d_1, d_2) which is the scale invariance range of the image pyramid.
    
  * Scoring smaller α_1(𝐓_pt𝐗_k, 𝐏_j^i) shown in Fig. <ref> which refers to the angle between camera's focal line n_c and ray line OP of the point. 
    
  * Scoring reward smaller α_2(𝐗_k, 𝐏_j^i) shown in Fig. <ref> which refers to the angle between mean view line n_p and ray line OP of the point.

According to these three principles, we evaluate every pan-tilt sample with M_l. At first, we eliminate the points that have a distance out of the range (d_1, d_2) or have an angle α_2 > 60^∘. The rest of the points in the local map make up a collection denoted S_r. For every map point 𝐏_j^i∈S_r, we calculate the score of a pan-tilt sample 𝐪 and optimize it by:

    𝐪^*=𝐪maxi,j,
    𝐏_j^i∈S_r∑cos(α_1)cos(α_2)


The maximum of the cosine functions is achieved when the angle is 0, which indicates that the best view angle is obtained when the focal line of the camera and the mean view angle of the map point overlap.




 §.§ Path Learning and Tracking with Active Camera


When the human guide finishes teaching the path, a complete map consisting of plenty of 3D map points and a graph of keyframes are saved by the system. The keyframes with timestamps and poses contain trajectory information of the active camera on this path. In the repeat phase, the map points are used to construct a local map according to the current pose of the camera. Every time a keyframe is stored in memory, the PTU sample is read from the angle encoder and saved with the same timestamp. In summary, the mobile robot is taught with a feature map and a camera trajectory consisting of keyframes with timestamps and corresponding PTU angles. Then the robot trajectory is computed from PTU samples and camera trajectory by inverse operation of Equation (<ref>):

    𝐗_k =  𝐓_pt^𝖳𝐗_k'


When the mobile robot is needed to navigate the learned path, the feature map taught in the teach phase is loaded at first. Then PnP algorithm<cit.> is used to find the initial pose of the robot. Once the robot has found its position in the map, the active camera-based SLAM shown in Fig. <ref> is booted up to run local map tracking for current pose 𝐗_k,w. Then we search the Keyframe poses for the closest reference pose X_r,w∈SE(3) in front of the robot. Finally the pose error between 𝐗_k,w and 𝐗_r,w is transferred into a PD controller<cit.> C_pd to compute the current velocity ϕ_k:

    ϕ_k = C_pd(𝐗_r,w-𝐗_k,w)

To speed up the searching of a reference keyframe, we define a window with a width of 10 that is centered at the last reference keyframe. In the repeat phase, the computer keeps outputting the motion calculated by Equation (<ref>) to the wheels.













§ EXPERIMENTS AND DISCUSSION

In both phases of VT&R, the active camera runs automatically according to current perception. In the teach phase, the robot runs a path following a human guide and automatically learns the path using the active camera-based VSLAM. In the repeat phase, the robot is placed anywhere near the road in a similar orientation to the taught one. Once started, the robot processes the images input by the active camera to localize and control the robot. It needs to be emphasized that the main purpose of our experiments is to demonstrate repeatable and successful VT&R on the difficult paths with our system. As the trajectory error in the repeat phase are all acceptable, we emphasize the results of completion rate, which indicates that only our FLAF-based Active VT&R can finish all three paths. It is worth noting that the UDVP-based active VT&R always fails in the same position and for the same reason in our repeated experiments.

Experiments are performed on three paths to evaluate our active view planning method and VT&R system. The first two paths are in the effective range of the motion capture device in the Shenzhen Key Laboratory of Robotics and Computer Vision. Path three leads the robot from an indoor location to a reading space out of the laboratory and finally back to the start. Experiments on each of the three paths are performed 10 times for consistent results. All the data in Table I are the average results of 10 repeated experiments. The trajectories shown in Fig. 5 and the point graph shown in Fig. 7 are a representative selection, considering that the traditional methods always fail in the same situation. In Fig. 6, we demonstrate the process of experiments on the most difficult path connecting different rooms.

We compare our FLAF-based active view planning with three other methods on the task of VT&R. "Passive VT&R" is achieved with our VT&R system without the PTU and active view planning. The "UDVP-based active VT&R" is a reproduction of the observation model proposed in <cit.> with our VT&R system. The only difference between FLAF and UDVP is the observation model. The equation(<ref>) is supported by the comparison of "FLAF-based active" with "FLAF by counting," which is the first version of FLAF that has been eliminated by us.






 §.§ Implementation and Experimental Setup


As shown in Figure <ref>(d), we fix an Intel Realsense D435 camera on an I-Quotient-Robotics PTU to make up our active camera. This active camera is mounted on a Clearpath-Jackal robot. Our active VT&R system runs in real-time on a notebook with an Intel i7 (2.3GHz) CPU and responds to the images exactly at the frame rate of 20Hz.

Ground truths of path one and two are obtained from motion capture in the teach phase. The ground truth of Path 3 is built by SfM, as there is no motion capture outside the laboratory. The visual repeating trajectories of all methods are compared with the ground truths and painted in Fig. 5. In Table I, the CR data shown indicates the VT&R completion rate of this view planning method, and the time data indicates the mean time used for the view planning methods implemented by sampling-based optimization. The efficacy of VSLAM is decreased by the relatively low speed of view planning compared to the SLAM speed of 20-30Hz. The translational RMSE of trajectories is computed using evo<cit.> by comparing the repeat trajectory with the taught one based on timestamps, resulting in greater error than physical truth. The trajectory consistencies between repeat and teach shown in Fig. <ref> are similar and all acceptable.  


On path one and two, we demonstrate the efficacy of our VT&R system in both an active and a passive way. On path three (Fig. <ref>), we show a challenging case with low-texture regions where passive VT&R fails and active VT&R succeeds. Additionally, our FLAF model is verified on all three paths to outperform the existing UDVP in repeating a complete path. FLAF by counting refers to counting the map points in a range defined by FLAF instead of grading the points by the product of the cosine functions shown in Equation (<ref>).





 §.§ Tracking Failure Avoidance Validation

As shown in Table <ref> and Fig. <ref>, the passive VT&R achieves a stable and accurate performance on the first two paths but fails in the teach phase on path three. The few low-texture regions on path one and two are avoided by a considerable human guide. Our active VT&R system with active view planning succeeds in the teach phase on three paths.

Fig. <ref> demonstrates the situations where low-texture regions are overcome by our active VT&R with an active camera that automatically captures informative regions to maintain a stable localization. At position one, the active camera automatically looks at the poster in the upper left to avoid the white wall. At position two, the active camera looks up at the ceiling to maintain the localization relying on the square lamps. The robot looks toward the upper right at position three to focus on the logo while passing through a low-texture corner. Finally, at position four, the robot looks toward the upper left at the scroll on the door for abundant features. 




 §.§ Active View Planning Method Evaluation


Active SLAM with different observation models typically succeeds in the teach phase on all paths. Because they mostly control the robot to see the partial map just built. This is achieved by looking at the directions that have more mapped features. However, existing methods such as UDVP<cit.> ignore the identifiability of the points by feature extractor and matcher. In the mapping phase active SLAM has no choice but to see the just mapped points because there are no more feature points triangulated. However, in the repeat phase, some of the map points (3D coordinates) can't be identified by VSLAM from a large view angle compared with the view direction in which they were triangulated.

Our FLAF-constrained active view planning outperforms the UDVP model in repeating a complete path because of our consideration of the affine change of the feature points. The map point's normal line (𝐧_p) shown in Fig. <ref> limits the orientation of the active camera in the view angle-invariance range of the feature. Table I shows that our method repeats a more complete trajectory on all paths, especially in some challenging cases. Although on path three UDVP achieves more accurate path tracking, the trajectory errors are all negligible for VT&R. The trajectories shown in Fig. <ref> also verify that the repeat consistency with different methods are all pretty good, which means the repeat trajectories always fit with the taught one. Part of the errors are caused by the time-wise point-to-point comparison. The timestamps are difficult to align. We just modified them to be uniformly consistent at the same time duration. Initially, we implement our FLAF model by counting the map points in the FoV of a PTU sample that accords with FLAF, which is more accurate for the higher speed. Only the final version of our FLAF-constrained active VT&R successfully finishes all three paths.







 §.§ Map Points Association Validation


In<cit.>, the authors draw a point graph to show the relationship between the probability of the tracking failure and the number of observed map points. This work<cit.> indicates that the probability of the tracking failure approaches zero if the observed map points are more than a threshold. Our work further indicates that the tracking failure is caused by failed local map tracking. We use the analyzing method proposed in<cit.> to analyze the state of map point association in the repeat phase. As shown in Fig. <ref>, we recorded the quantity of matched points in local map tracking and drew the line graph of two methods for all three paths. 

On path one, the active camera-based VSLAM with FLAF matches fewer points than it is by UDVP initially. However, the VSLAM with UDVP fails to track the path after a reduction of matched points. On the contrary, the VSLAM with our proposed FLAF-based method initially tracks fewer points of the local map but maintains a stable localization and increasing number of matches. On path three, the active visual repeat with these two view planning methods tracks a similar quantity of map points, but our method recovers the localization after a challenging decline where the UDVP-based method fails. 

On path three, a similar case happens that the UDVP method loses the localization after a reduction of matched map points. In the same challenging location, our FLAF-based method finishes repeating the path by adjusting the active camera to the direction with more identifiable map points. Fig. <ref> also shows that the UDVP method controls the active camera to see the direction with more local map points without considering the feature identifiability of these points. It means although these points are in the FoV of the camera, they can not be identified and matched by the feature extractor and matcher because of the ignorance of the affine change of the points. 



§ CONCLUSION




 §.§ Conclusions

In this research, we present a novel active view planning method for VT&R to solve the tracking failure caused by low-texture regions. Our passive VT&R system is built on feature-based VSLAM and faces the main problem of tracking failure caused by low-texture regions. Existing feature-based active VT&R systems use a gimbal-stabilized camera to improve the stability. The gimbal is typically manually steered in the teach phase instead of using active view planning. Previous works propose some active view planning methods for VSLAM. However, these works haven't demonstrated how to use these methods to help VT&R with an active camera. 

We aim to use an active camera with active view planning to build up an active VSLAM system that can support our active VT&R system. After our test, a recent active view planning method for VSLAM succeeds in the teach phase but easily fails in the repeat phase. This method controls the camera toward directions with more points close to the camera. However, it fails to track the local map in cases when the closer points are not identifiable by the ORB feature extractor and matcher. Therefore, we design a focal line and feature (FLAF)-constrained  active view planning method taking into account the affine change of feature points. 

According to our experimental results, passive VT&R fails with the low-texture regions. Active camera-based VSLAM with an existing view planning method easily fails in the repeat phase because it doesn't account for view angle-dependent affine change of the features. With our proposed FLAF model, the novel active view planning method successfully helps our VT&R system finish all three paths. Our experimental results also demonstrate the process of overcoming the low-texture regions with an active camera. Finally, we analyze the reason for tracking failure through three graphs that show the relationship between the quantity of matched points and time.

Our most important contribution is the demonstration of active camera-based VT&R. We focus on solving the problem of tracking failure in VT&R caused by low-texture regions. We will address the dilemma between exploitation of the existing map and exploration of the unknown environment in the future.


00

b1 Paul Furgale, and Timothy D Barfoot, “Visual teach and repeat for long-range rover autonomy,” Journal of Field Robotics, 2010.

b2 Michael Paton, Kirk MacTavish, Michael Warren, Timothy D Barfoot, “Bridging the appearance gap: Multi-experience localization for long-term visual teach and repeat.” In IEEE/RSJ Int. Conf. on Intell. Robots and Systems, 2016.

b3 L. Peterson, D. Austin, and D. Kragic, "High-level control of a mobile manipulator for door opening”, In IEEE/RSJ Int. Conf. on Intell. Robots and Systems, 2000.

b4 Mohit Mehndiratta and Erdal Kayacan, "A constrained instantaneous learning approach for aerial package delivery robots: onboard implementation and experimental results," Autonomous Robots, 2019.

b5 Guillaume Bresson, Zayed Alsayed, Li Yu, and Sébastien Glaser, “Simultaneous Localization and Mapping: A Survey of Current Trends in Autonomous Driving,” IEEE Trans. Intell. Vehicles, 2017.

b6 H Ye, G Chen, W Chen, L He, Y Guan, and H Zhang, "Mapping While Following: 2D LiDAR SLAM in Indoor Dynamic Environments with a Person Tracker," In IEEE Int. Conf. on Robot. and Biomimetics, 2021.


b7 Jakob Engel, Vladlen Koltun, and Daniel Cremers. "Direct sparse odometry." IEEE Trans. Pattern Anal. Mach. Intell, 2017.

b8 Christian Forster, Matia Pizzoli, and Davide Scaramuzza. "SVO: Fast semi-direct monocular visual odometry." In IEEE Int. Conf. on Robotics and Automation, 2014.


b9 R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “Orb-slam: a versatile and accurate monocular slam system,” IEEE Trans. Robotics, 2015.

b10David G Lowe. "Distinctive image features from scale-invariant keypoints." International journal of Computer Vision, 2004.

b11 Seyed Abbas Sadat, Kyle Chutskoff, Damir Jungic, Jens Wawerla and Richard Vaughan, "Feature-Rich Path Planning for Robust Navigation of MAVs with Mono-SLAM," In IEEE Int. Conf. on Robotics and Automation, 2014.

b12 Xinke Deng, Zixu Zhang, Avishai Sintov, Jing Huang, and Timothy Bretl, "Feature-constrained Active VSLAM for Mobile Robot Navigation," In IEEE Int. Conf. on Robotics and Automation, 2018.




b14 Matias Mattamala, Milad Ramezani, Marco Camurri, and Maurice Fallon. "Learning camera performance models for active multi-camera visual teach and repeat." In IEEE Int. Conf. on Robotics and Automation, 2021.

b15 Simone Frintrop, and Patric Jensfelt. "Attentional landmarks and active gaze control for VSLAM." IEEE Trans. Robotics, 2008.

b16 Andrew J. Davison and David W. Murray. "Mobile robot localisation using active vision." In European Conference on Computer Vision, 1998.

b17 Xu-Yang Dai, Qing-Hao Meng, and Sheng Jin. "Uncertainty-driven active view planning in feature-based monocular vSLAM." Applied Soft Computing, 2021.


b18 Michael Warren, Angela P. Schoellig, and Timothy D. Barfoot. "Level-headed: Evaluating gimbal-stabilised visual teach and repeat for improved localisation performance." In IEEE Int. Conf. on Robotics and Automation, 2018.

b19 Hauke Strasdat, José MM Montiel, and Andrew J. Davison. "VSLAM: why filter?" Image and Vision Computing, 2012.


b20 Weinan Chen, Changfei Fu, Lei Zhu, Shing-Yan Loo, and Hong Zhang. “Rumination Meets VSLAM: You Do Not Need to Build All the Submaps in Realtime.” IEEE Trans. Industrial Electronics, 2023.

b21 Etienne Mouragnon, Maxime Lhuillier, Michel Dhome, Fabien Dekeyser, and Patrick Sayd." Real time localization and 3d reconstruction." In IEEE Int. Conf. on Computer Vision and Pattern Recognition, 2006.


b22 Ethan Rublee, Vincent Rabaud, Kurt Konolige, and Gary Bradski. "ORB: An efficient alternative to SIFT or SURF." In IEEE Int. Conf. on Computer Vision, 2011.

b23 Vincent Lepetit, Francesc Moreno-Noguer, and Pascal Fua. "EPnP: An accurate O(n) solution to the PnP problem." International Journal of Computer Vision, 2009.

b24 Weinan Chen, Lei Zhu, Xubin Lin, Li He, Yisheng Guan, and Hong Zhang. "Dynamic strategy of keyframe selection with pd controller for vslam systems." IEEE/ASME Trans. Mechatronics, 2021.

b25 Michael Grupp. "evo: Python package for the evaluation of odometry and slam." 2017, Available: https://github.com/MichaelGrupp/evo.

b26 Christian Mostegel, Andreas Wendel, and Horst Bischof. "Active monocular localization: Towards autonomous monocular exploration for multirotor mavs." In IEEE Int. Conf on Robotics and Automation, 2014.





