Visual servoing involves choosing actions that move a robot in response to observations from a camera, in order to reach a goal configuration in the world. Standard visual servoing approaches typically rely on manually designed features and analytical dynamics models, which limits their generalization capability and often requires extensive application-specific feature and model engineering. In this work, we study how learned visual features, learned predictive dynamics models, and reinforcement learning can be combined to learn visual servoing mechanisms. We focus on target following, with the goal of designing algorithms that can learn a visual servo using low amounts of data of the target in question, to enable quick adaptation to new targets. Our approach is based on servoing the camera in the space of learned visual features, rather than image pixels or manually-designed keypoints. We demonstrate that standard deep features, in our case taken from a model trained for object classification, can be used together with a bilinear predictive model to learn an effective visual servo that is robust to visual variation, changes in viewing angle and appearance, and occlusions. A key component of our approach is to use a sample-efficient fitted Q-iteration algorithm to learn which features are best suited for the task at hand. We show that we can learn an effective visual servo on a complex synthetic car following benchmark using just 20 training trajectory samples for reinforcement learning. We demonstrate substantial improvement over a conventional approach based on image pixels or hand-designed keypoints, and we show an improvement in sample-efficiency of more than two orders of magnitude over standard model-free deep reinforcement learning algorithms. Videos are available at http://rll.berkeley.edu/visual_servoing.
1 INTRODUCTION
Visual servoing is a classic problem in robotics that requires moving a camera or robot to match a target configuration of visual features or image intensities. Many robot control tasks that combine perception and action can be posed as visual servoing, including navigation (DeSouza & Kak, 2002; Chen et al., 2006), where a robot must follow a desired path; manipulation, where the robot must servo an end-effector or a camera to a target object to grasp or manipulate it (Malis et al., 1999; Corke, 1993; Hashimoto, 1993; Hosoda & Asada, 1994; Kragic & Christensen, 2002); and various other problems, as surveyed in Hutchinson et al. (1996). Most visual servoing methods assume access to good geometric image features (Chaumette & Hutchinson, 2006; Collewet et al., 2008; Caron et al., 2013) and require knowledge of their dynamics, which are typically obtained from domain knowledge about the system. Using such hand-designed features and models prevents exploitation of statistical regularities in the world, and requires manual engineering for each new system.
In this work, we study how learned visual features, learned predictive dynamics models, and reinforcement learning can be combined to learn visual servoing mechanisms. We focus on target following, with the goal of designing algorithms that can learn a visual servo using low amounts of
data of the target in question, so as to be easy and quick to adapt to new targets. Successful target following requires the visual servo to tolerate moderate variation in the appearance of the target, including changes in viewpoint and lighting, as well as occlusions. Learning invariances to all such distractors typically requires a considerable amount of data. However, since a visual servo is typically specific to a particular task, it is desirable to be able to learn the servoing mechanism very quickly, using a minimum amount of data. Prior work has shown that the features learned by large convolutional neural networks on large image datasets, such as ImageNet classification (Deng et al., 2009), tend to be useful for a wide range of other visual tasks (Donahue et al., 2014). We explore whether the usefulness of such features extends to visual servoing.
To answer this question, we propose a visual servoing method that uses pre-trained features, in our case obtained from the VGG network (Simonyan & Zisserman, 2014) trained for ImageNet classification. Besides the visual features, our method uses an estimate of the feature dynamics in visual space by means of a bilinear model. This allows the visual servo to predict how motion of the robot’s camera will affect the perceived feature values. Unfortunately, servoing directly on the high-dimensional features of a pre-trained network is insufficient by itself to impart robustness on the servo: the visual servo must not only be robust to moderate visual variation, but it must also be able to pick out the target of interest (such as a car that the robot is tasked with following) from irrelevant distractor objects. To that end, we propose a sample-efficient fitted Q-iteration procedure that automatically chooses weights for the most relevant visual features. Crucially, the actual servoing mechanism in our approach is extremely simple, and simply seeks to minimize the Euclidean distance between the weighted feature values at the next time step and the target. The form of the servoing policy in our approach leads to an analytic and tractable linear approximator for the Qfunction, which leads to a computationally efficient fitted Q-iteration algorithm. We show that we can learn an effective visual servo on a complex synthetic car following benchmark using just 20 training trajectory samples for reinforcement learning. We demonstrate substantial improvement over a conventional approach based on image pixels or hand-designed keypoints, and we show an improvement in sample-efficiency of more than two orders of magnitude over standard model-free deep reinforcement learning algorithms.
The environment for the synthetic car following benchmark is available online as the package CitySim3D1, and the code to reproduce our method and experiments is also available online2. Supplementary videos of all the test executions are available on the project’s website3.
2 RELATED WORK
Visual servoing is typically (but not always) performed with calibrated cameras and carefully designed visual features. Ideal features for servoing should be stable and discriminative, and much of the work on visual servoing focuses on designing stable and convergent controllers under the assumption that such features are available (Espiau et al., 2002; Mohta et al., 2014; Wilson et al., 1996). Some visual servoing methods do not require camera calibration (Jagersand et al., 1997; Yoshimi & Allen, 1994), and some recent methods operate directly on image intensities (Caron et al., 2013), but generally do not use learning to exploit statistical regularities in the world and improve robustness to distractors.
Learning is a relatively recent addition to the repertoire of visual servoing tools. Several methods have been proposed that apply ideas from reinforcement learning to directly acquire visual servoing controllers (Lampe & Riedmiller, 2013; Sadeghzadeh et al., 2015). However, such methods have not been demonstrated under extensive visual variation, and do not make use of state-of-the-art convolutional neural network visual features. Though more standard deep reinforcement learning methods (Lange et al., 2012; Mnih et al., 2013; Levine et al., 2016; Lillicrap et al., 2015) could in principle be applied to directly learn visual servoing policies, such methods tend to require large numbers of samples to learn task-specific behaviors, making them poorly suited for a flexible visual servoing algorithm that can be quickly repurposed to new tasks (e.g. to following a different object).
1https://github.com/alexlee-gk/citysim3d 2https://github.com/alexlee-gk/visual_dynamics 3http://rll.berkeley.edu/visual_servoing
Instead, we propose an approach that combines learning of predictive models with pre-trained visual features. We use visual features trained for ImageNet (Deng et al., 2009) classification, though any pre-trained features could in principle be applicable for our method, so long as they provide a suitable degree of invariance to visual distractors such as lighting, occlusion, and changes in viewpoint. Using pre-trained features allows us to avoid the need for large amounts of experience, but we must still learn the policy itself. To further accelerate this process, we first acquire a predictive model that allows the visual servo to determine how the visual features will change in response to an action. General video prediction is an active research area, with a number of complex but data-hungry models proposed in recent years (Oh et al., 2015; Watter et al., 2015; Mathieu et al., 2015; Xue et al., 2016; Lotter et al., 2016; Jia et al., 2016; Walker et al., 2016; Vondrick et al., 2016).
However, we observe that convolutional response maps can be interpreted as images and, under mild assumptions, the dynamics of image pixels during camera motion can be well approximated by means of a bilinear model (Censi & Murray, 2015). We therefore train a relatively simple bilinear model for short-term prediction of visual feature dynamics, which we can use inside a very simple visual servo that seeks to minimize the error between the next predicted feature values and a target image.
Unfortunately, simply training predictive models on top of pre-trained features is insufficient to produce an effective visual servo, since it weights the errors of distractor objects the same amount as the object of interest. We address this challenge by using an efficient Q-iteration algorithm to train the weights on the features to maximize the servo’s long-horizon reward. This method draws on ideas from regularized fitted Q-iteration (Gordon, 1995; Ernst et al., 2005; Farahmand et al., 2009) and neural fitted Q-iteration (Riedmiller, 2005) to develop a sample-efficient algorithm that can directly estimate the expected return of the visual servo without the use of any additional function approximator.
3 PROBLEM STATEMENT
Let yt be a featurization of the camera’s observations xt and let y∗ be some given goal feature map. For the purposes of this work, we define visual servoing as the problem of choosing controls ut for a fixed number of discrete time steps t as to minimize the error ‖y∗ − yt‖. We use a relatively simple gradient-based servoing policy that uses one-step feature dynamics, f : {yt,ut} → yt+1. The policy chooses the control that minimizes the distance between the goal feature map and the one-step prediction:
π(xt,x∗) = argmin u ‖y∗ − f(yt,u)‖2 . (1)
Learning this policy amounts to learning the robot dynamics and the distance metric ‖·‖. To learn the robot dynamics, we assume that we have access to a dataset of paired observations and controls xt,ut,xt+1. This data is relatively easy to obtain as it involves collecting a stream of the robot’s observations and controls. We use this dataset to learn a general visual dynamics model that can be used for any task.
To learn the distance metric, we assume that the robot interacts with the world and collects tuples of the form xt,ut, ct,xt+1,x∗. At every time step during learning, the robot observes xt and takes action ut. After the transition, the robot observes xt+1 and receives an immediate cost ct. This cost is task-specific and it quantifies how good that transition was in order to achieve the goal. At the beginning of each trajectory, the robot is given a goal observation x∗, and it is the same throughout the trajectory. We define the goal feature map to be the featurization of the goal observation. We learn the distance metric using reinforcement learning and we model the environment as a Markov Decision Process (MDP). The state of the MDP is the tuple of the current observation and the episode’s target observation, st = (xt,x∗), the action ut is the discrete-time continuous control of the robot, and the cost function maps the states and action (st,ut, st+1) to a scalar cost ct.
4 VISUAL FEATURES DYNAMICS
We learn a multiscale bilinear model to predict the visual features of the next frame given the current image from the robot’s camera and the action of the robot. An overview of the model is shown in Figure 1. The learned dynamics can then be used for visual servoing as described in Section 5.
4.1 VISUAL FEATURES
We consider both pixels and semantic features for the visual representation. We define the function h to relate the image x and its feature y = h (x). Our choice of semantic features are derived from the VGG-16 network (Simonyan & Zisserman, 2014), which is a convolutional neural network trained for large-scale image recognition on the ImageNet dataset (Deng et al., 2009). Since spatial invariance is undesirable for servoing, we remove some of the max-pooling layers and replace the convolutions that followed them with dilated convolutions, as done by Yu & Koltun (2015). The modified VGG network is shown in Figure 2. We use the model weights of the original VGG-16 network, which are publicly available as a Caffe model (Jia et al., 2014). The features that we use are the outputs of some of the intermediate convolutional layers, that have been downsampled to a 32× 32 resolution (if necessary) and standarized with respect to our training set. We use multiple resolutions of these features for servoing. The idea is that the high-resolution representations have detailed local information about the scene, while the low-resolution representations have more global information available through the image-space gradients. The features at level l of the multiscale pyramid are denoted as y(l). The features at each level are obtained from the features below through a downsampling operator d(y(l−1)) = y(l) that cuts the resolution in half.
4.2 BILINEAR DYNAMICS
The features y(l)t are used to predict the corresponding level’s features y (l) t+1 at the next time step, conditioned on the action ut, according to a prediction function f (l)(y (l) t ,ut) = ŷ (l) t+1. We use a bilinear model to represent these dynamics, motivated by prior work (Censi & Murray, 2015). In order to servo at different scales, we learn a bilinear dynamics model at each scale. We consider two variants of the bilinear model in previous work in order to reduce the number of model parameters.
The first variant uses fully connected dynamics as in previous work but models the dynamics of each channel independently. When semantic features are used, this model interprets the feature maps as
being abstract images with spatial information within a channel and different entities or factors of variation across different channels. This could potentially allow the model to handle moving objects, occlusions, and other complex phenomena.
The fully connected bilinear model is quite large, so we propose a bilinear dynamics that enforces sparsity in the parameters. In particular, we constrain the prediction to depend only on the features that are in its local spatial neighborhood, leading to the following locally connected bilinear model:
ŷ (l) t+1,c = y (l) t,c + ∑ j ( W (l) c,j ∗ y (l) t,c +B (l) c,j ) ut,j + ( W (l) c,0 ∗ y (l) t,c +B (l) c,0 ) . (2)
The parameters are the 4-dimensional tensor W (l)c,j and the matrix B (l) c,j for each channel c, scale l, and control coordinate j. The last two terms are biases that allow to model action-independent visual changes, such as moving objects. The ∗ is the locally connected operator, which is like a convolution but with untied filter weights4.
4.3 TRAINING VISUAL FEATURE DYNAMICS MODELS
The loss that we use for training the bilinear dynamics is the sum of the losses of the predicted features at each level, ∑L l=0 `
(l), where the loss for each level l is the squared `-2 norm between the predicted features and the actual features of that level, `(l) = ‖y(l)t+1 − ŷ (l) t+1‖2.
We optimize for the dynamics while keeping the feature representation fixed. This is a supervised learning problem, which we solve with ADAM (Kingma & Ba, 2014). The training set, consisting of triplets xt,ut,xt+1, was obtained by executing a hand-coded policy that moves the robot around the target with some Gaussian noise.
5 LEARNING VISUAL SERVOING WITH REINFORCEMENT LEARNING
We propose to use a multiscale representation of semantic features for servoing. The challenge when introducing multiple scales and multi-channel feature maps for servoing is that the features do not necessarily agree on the optimal action when the goal is unattainable or the robot is far away from the goal. To do well, it’s important to use a good weighing of each of the terms in the objective. Since there are many weights, it would be impractically time-consuming to set them by hand, so we resort to learning. We want the weighted one-step lookahead objective to encourage good longterm behavior, so we want this objective to correspond to the state-action value function Q. So we propose a method for learning the weights based on fitted Q-iteration.
5.1 SERVOING WITH WEIGHTED MULTISCALE FEATURES
Instead of attempting to build an accurate predictive model for multi-step planning, we use the simple greedy servoing method in Equation (1), where we minimize the error between the target and predicted features for all the scales. Typically, only a few objects in the scene are relevant, so the errors of some channels should be penalized more than others. Similarly, features at different scales might need to be weighted differently. Thus, we use a weighting w(l)c ≥ 0 per channel c and scale l:
π(xt,x∗) = argmin u ∑ c L∑ l=0 w (l) c |y(l)·,c | ∥∥∥y(l)∗,c − f (l)c (y(l)t,c,u)∥∥∥2 2 + ∑ j λju 2 j , (3)
where | · | denotes the cardinality operator and the constant 1/|y(l)·,c| normalizes the feature errors by its spatial resolution. We also use a separate weight λj for each control coordinate j. This optimization can be solved efficiently since the dynamics is linear in the controls (see Appendix A).
4 The locally connected operator, with a local neighborhood of nf × nf (analogous to the filter size in convolutions), is defined as:
(W ∗ y)kh,kw = kh+bnf/2c∑
ih=kh−bnf/2c kw+bnf/2c∑ iw=kw−bnf/2c Wkh,kw,ih−kh,iw−kwyih,iw .
5.2 Q-FUNCTION APPROXIMATION FOR THE WEIGHTED SERVOING POLICY
We choose a Q-value function approximator that can represent the servoing objective such that the greedy policy with respect to the Q-values results in the policy of Equation (3). In particular, we use a function approximator that is linear in the weight parameters θ> = [ w> λ> ] :
Qθ,b(st,u) = φ(st,u) >θ + b, φ (st,u) > =
[[ 1
|y(l)·,c| ∥∥∥y(l)∗,c − f (l)c (y(l)t,c,u)∥∥∥2 2 ]> c,l [ u2j ]> j ] .
We denote the state of the MDP as st = (xt,x∗) and add a bias b to the Q-function. The servoing policy is then simply πθ(st) = argminu Qθ,b(st,u). For reinforcement learning, we optimized for the weights θ but kept the feature representation and its dynamics fixed.
5.3 LEARNING THE Q-FUNCTION WITH FITTED Q-ITERATION
Reinforcement learning methods that learn a Q-function do so by minimizing the Bellman error:∥∥∥∥Q (st,ut)− (ct + γminu Q (st+1,u) )∥∥∥∥2
2
. (4)
In fitted Q-iteration, the agent iteratively gathers a dataset {s(i)t ,u(i)t , c(i)t , s(i)t+1}Ni of N samples according to an exploration policy, and then minimizes the Bellman error using this dataset. We use the term sampling iteration to refer to each iteration j of this procedure. At the beginning of each sampling iteration, the current policy with added Gaussian noise is used as the exploration policy.
It is typically hard or unstable to optimize for both Q-functions that appear in the Bellman error of Equation (4), so it is usually optimized by iteratively optimizing the current Q-function while keeping the target Q-function constant. However, we notice that for a given state, the action that minimizes its Q-values is the same for any non-negative scaling α of θ and for any bias b. Thus, to speed up the optimization of the Q-function, we first set α(k− 1 2 ) and b(k− 1 2 ) by jointly solving for α and b of both the current and target Q-function:
min α≥0,b
1
N N∑ i=1 ∥∥∥∥Qαθ(k−1),b (s(i)t ,u(i)t )− (c(i)t + γminu Qαθ(k−1),b (s(i)t+1,u) )∥∥∥∥2 2 + ν ‖θ‖22 . (5)
This is similar to how, in policy evaluation, state values can be computed by solving a linear system. We regularize the parameters with an `-2 penalty, weighted by ν ≥ 0. We use the term FQI iteration to refer to each iteration k of optimizing the Bellman error, and we use the notation (k− 12 ) to denote an intermediate step between iterations (k−1) and (k). The parameters θ can then be updated with θ(k− 1 2 ) = α(k− 1 2 )θ(k−1). Then, we update θ(k) and b(k) by optimizing for θ and b of the current Q-function while keeping the parameters of the target Q-function fixed:
min θ≥0,b
1
N N∑ i=1 ∥∥∥∥Qθ,b (s(i)t ,u(i)t )− (c(i)t + γminu Qθ(k− 12 ),b(k− 12 ) (s(i)t+1,u) )∥∥∥∥2 2 + ν ‖θ‖22 . (6)
A summary of the algorithm used to learn the feature weights is shown in Algorithm 1.
Algorithm 1 FQI with initialization of policy-independent parameters
1: procedure FQI(θ(0), σ2exploration, ν) 2: for s = 1, . . . , S do . sampling iterations 3: Gather dataset {s(i)t ,u(i)t , c(i)t , s(i)t+1}Ni using exploration policy N (πθ(0) , σ2exploration) 4: for k = 1, . . . ,K do . FQI iterations 5: Fit α(k− 1 2 ) and b(k− 1 2 ) using (5) 6: θ(k− 1 2 ) ← α(k− 12 )θ(k−1) 7: Fit θ(k) and b(k) using (6) 8: θ(0) ← θ(K)
6 EXPERIMENTS
We evaluate the performance of the model for visual servoing in a simulated environment. The simulated quadcopter is governed by rigid body dynamics. The robot has 4 degrees of freedom, corresponding to translation along three axis and yaw angle. This simulation is inspired by tasks in which an autonomous quadcopter flies above a city, with the goal of following some target object (e.g., a car).
6.1 LEARNING FEATURE DYNAMICS AND WEIGHTS WITH FQI
The dynamics for each of the features were trained using a dataset of 10000 samples (corresponding to 100 trajectories) with ADAM (Kingma & Ba, 2014). A single dynamics model was learned for each feature representation for all the training cars (Figure 3). This training set was generated by executing a hand-coded policy that navigates the quadcopter around a car for 100 time steps per trajectory, while the car moves around the city.
We used the proposed FQI algorithm to learn the weightings of the features and control regularizer. At every sampling iteration, the current policy was executed with Gaussian noise to gather data from 10 trajectories. All the trajectories in our experiments were up to 100 time steps long. The immediate cost received by the agent encodes the error of the target in image coordinates (details in Appendix B). Then, the parameters were iteratively updated by running K = 10 iterations of FQI. We ran the overall algorithm for only S = 2 sampling iterations and chose the parameters that achieved the best performance on 10 validation trajectories. These validation trajectories were obtained by randomly choosing 10 cars from the set of training cars and randomly sampling initial states, and executing the policy with the parameters of the current iteration. All the experiments share the same set of validation trajectories.
Feature Dynamics Observations from Test Executions Cost
6.2 COMPARISON OF FEATURE REPRESENTATIONS FOR SERVOING
We compare the servoing performance for various feature dynamics models, where the weights are optimized with FQI. We execute the learned policies on 100 test trajectories and report the average cost of the trajectory rollouts on Figure 5. The cost of a single trajectory is the (undiscounted) sum of costs ct. We test the policies with cars that were seen during training as well as with a set of novel cars (Figure 4), to evaluate the generalization of the learned dynamics and optimized policies.
The test trajectories were obtained by randomly sampling 100 cars (with replacement) from one of the two sets of cars, and randomly sampling initial states (which are different from the ones used for validation). For consistency and reproducibility, the same sampled cars and initial states were used across all the test experiments, and the same initial states were used for both sets of cars. These test trajectories were never used during the development of the algorithm or for choosing hyperparameters.
From these results, we notice that policies based on deeper VGG features, up to VGG conv4 3, generally achieve better performance. However, the deepest feature representation, VGG conv5 3, is not as suitable for approximating Q-values. We hypothesize that this feature might be too spatially invariant and it might lack the necessary spatial information to differentiate among different car positions. The policies based on pixel intensities and VGG conv5 3 features perform worse on the novel cars. However, VGG features conv1 2 through conv4 3 achieve some degree of generalization on the novel cars.
We show sample trajectories in Table 1. The policy based on pixel-intensities is susceptible to occlusions and distractor objects that appear in the target image or during executions. This is because distinguishing these occlusions and distractors from the cars cannot be done using just RGB features.
6.3 COMPARISON OF WEIGHTINGS FROM OTHER OPTIMIZATION METHODS
We compare our policy using conv4 3 feature dynamics, with weights optimized by FQI, against policies that use these dynamics but with either no feature weighting or weights optimized by other algorithms.
For the case of no weighting, we use a single feature weight w but optimize the relative weighting of the controls λ with the cross entropy method (CEM) (De Boer et al., 2005). For the other cases, we learn the weights with Trust Region Policy Optimization (TRPO) (Schulman et al., 2015). Since the servoing policy is the minimizer of a quadratic objective (Equation (3)), we represent the policy as a neural network that has a matrix inverse operation at the output. We train this network for 2 and 50 sampling iterations, and use a batch size of 4000 samples per iteration. All of these methods use the same feature representation as ours, the only difference being how the weights w and λ are chosen.
We report the average costs of these methods on the right of Figure 6. In 2 sampling iterations, the policy learned with TRPO does not improve by much, whereas our policy learned with FQI significantly outperforms the other policies. The policy learned with TRPO improves further in 50 iterations; however, the cost incurred by this policy is still about one and a half times the cost of our policy, despite using more than 100 times as many trajectories.
6.4 COMPARISON TO PRIOR METHODS
We also consider other methods that do not use the dynamics-based servoing policy that we propose. We report their average performance on the left of Figure 6.
For one of the prior methods, we train a convolutional neural network (CNN) policy end-to-end with TRPO. The policy is parametrized as a 5-layer CNN, consisting of 2 convolutional and 3 fullyconnected layers, with ReLU activations except for the output layer; the convolutional layers use
16 filters (4 × 4, stride 2) each and the first 2 fully-connected layers use 32 hidden units each. The policy takes in raw pixel-intensities and outputs controls.
This policy achieves a modest performance (although still worse than the policies based on conv4 3 feature dynamics) but it requires significantly more training samples than any of the other learningbased methods. We also trained CNN policies that take in extracted VGG features (without any dynamics) as inputs, but they perform worse (see Table 4 in the Appendix). This suggests that given a policy parametrization that is expressive enough and given a large number of training samples, it is better to directly provide the raw pixel-intensity images to the policy instead of extracted VGG features. This is because VGG features are not optimized for this task and their representation loses some information that is useful for servoing.
The other two prior methods use classical image-based visual servoing (IBVS) (Chaumette & Hutchinson, 2006) with respect to Oriented FAST and Rotated BRIEF (ORB) feature points (Rublee et al., 2011), or feature points extracted from a visual tracker. For the former, the target features consist of only the ORB feature points that belong to the car, and this specifies that the car is relevant for the task. For the tracker-based method, we use the Continuous Convolution Operator Tracker (C-COT) (Danelljan et al., 2016) (the current state-of-the-art visual tracker) to get bounding boxes around the car and use the four corners of the box as the feature points for servoing. We provide the ground truth car’s bounding box of the first frame as an input to the C-COT tracker. For all of the IBVS methods, we provide the ground truth depth values of the feature points, which are used in the algorithm’s interaction matrix5.
The first method performs poorly, in part because ORB features are not discriminative enough for some of the cars, and the target feature points are sometimes matched to feature points that are not on the car. The tracker-based method achieves a relatively good performance. The gap in performance with respect to our method is in part due to the lack of car dynamics information in the IBVS model, whereas our method implicitly incorporates that in the learned feature dynamics. It is also worth noting that the tracker-based policy runs significantly slower than our method. The open-source implementation of the C-COT tracker6 runs at about 1Hz whereas our policy based on conv4 3 features runs at about 16Hz. Most of the computation time of our method is spent computing features from the VGG network, so there is room for speedups if we use a network that is less computationally demanding.
7 DISCUSSION
Manual design of visual features and dynamics models can limit the applicability of visual servoing approaches. We described an approach that combines learned visual features with learning predictive dynamics models and reinforcement learning to learn visual servoing mechanisms. Our experiments demonstrate that standard deep features, in our case taken from a model trained for object classification, can be used together with a bilinear predictive model to learn an effective visual servo that is robust to visual variation, changes in viewing angle and appearance, and occlusions. For control we propose to learn Q-values, building on fitted Q-iteration, which at execution time allows for one-step lookahead calculations that optimize long term objectives. Our method can learn an effective visual servo on a complex synthetic car following benchmark using just 20 training trajectory samples for reinforcement learning. We demonstrate substantial improvement over a conventional approach based on image pixels or hand-designed keypoints, and we show an improvement in sample-efficiency of more than two orders of magnitude over standard model-free deep reinforcement learning algorithms.
ACKNOWLEDGEMENTS
This research was funded in part by the Army Research Office through the MAST program and the Berkeley DeepDrive consortium. Alex Lee was also supported by the NSF GRFP.
5The term interaction matrix, or feature Jacobian, is used in the visual servo literature to denote the Jacobian of the features with respect to the control.
6https://github.com/martin-danelljan/Continuous-ConvOp
A LINEARIZATION OF THE BILINEAR DYNAMICS
The optimization of Equation (3) can be solved efficiently by using a linearization of the dynamics,
f (l)c ( y (l) t,c,u ) = f (l)c ( y (l) t,c, ū ) + J (l) t,c (u − ū) = f (l)c ( y (l) t,c,0 ) + J (l) t,cu, (7)
where J (l)t,c is the Jacobian matrix with partial derivatives ∂f(l)c ∂u (y (l) t,c, ū) and ū is the linearization point. Since the bilinear dynamics are linear with respect to the controls, this linearization is exact and the Jacobian matrix does not depend on ū. Without loss of generality, we set ū = 0.
Furthermore, the bilinear dynamics allows the Jacobian matrix to be computed efficiently by simply doing a forward pass through the model. For the locally bilinear dynamics of Equation (2), the j-th column of the Jacobian matrix is given by
J (l) t,c,j =
∂f (l) c
∂uj (y
(l) t,c,0) =W (l) c,j ∗ y (l) t,c +B (l) c,j . (8)
B SERVOING COST FUNCTION FOR REINFORCEMENT LEARNING
The goal of reinforcement learning is to find a policy that maximizes the expected sum of rewards, or equivalently, a policy that minimizes the expected sum of costs. The cost should be one that quantifies progress towards the goal. We define the cost function in terms of the position of the target object (in the camera’s local frame) after the action has been taken,
c(st,ut, st+1) =  √( pxt+1 pzt+1 )2 + ( pyt+1 pzt+1 )2 + ( 1 pzt+1 − 1pz∗ )2 , if ||pt+1||2 ≥ τ and car in FOV
(T − t+ 1) c(·, ·, st), otherwise, (9)
where T is the maximum trajectory length. The episode terminates early if the camera is too close to the car (less than a distance τ ) or the car’s origin is outside the camera’s field of view (FOV). The car’s position at time t is pt = (p x t ,p y t ,p z t ) and the car’s target position is p∗ = (0, 0,p z ∗), both in the camera’s local frame (z-direction is forward). Our experiments use T = 100 and τ = 4m.
C EXPERIMENT DETAILS
C.1 TASK SETUP
The camera is attached to the vehicle slightly in front of the robot’s origin and facing down at an angle of π/6 rad, similar to a commercial quadcopter drone. The robot has 4 degrees of freedom, corresponding to translation and yaw angle. Pitch and roll are held fixed.
In our simulations, the quadcopter follows a car that drives at 1m s−1 along city roads during training and testing. The quadcopter’s speed is limited to within 10m s−1 for each translational degree of freedom, and its angular speed is limited to within π/2 rad s−1. The simulator runs at 10Hz. For each trajectory, a car is chosen randomly from a set of cars, and placed randomly on one of the roads. The quadcopter is initialized right behind the car, in the desired relative position for following. The image observed at the beginning of the trajectory is used as the goal observation.
C.2 LEARNING FEATURE DYNAMICS
The dynamics of all the features were trained using a dataset of 10000 triplets xt,ut,xt+1. The observations are 128× 128 RGB images and the actions are 4-dimensional vectors of real numbers encoding the linear and angular (yaw) velocities. The actions are normalized to between −1 and 1. The training set was generated from 100 trajectories of a quadcopter following a car around the city with some randomness. Each trajectory was 100 steps long. Only 5 training cars were shown during learning. The generation process of each trajectory is as follows: First, a car is chosen at random from the set of available cars and it is randomly placed on one of the roads. Then, the quadcopter is placed at some random position relative to the car’s horizontal pose, which is the car’s pose that has been rotated so that the vertical axis of it and the world matches. This quadcopter position is uniformly sampled in cylindrical coordinates relative to the car’s horizontal pose, with heights in the interval 12m to 18m, and azimuthal angles in the interval −π/2 rad to π/2 rad (where the origin of the azimuthal angle is the back of the car). The radii and yaw angles are initialized so that the car is in the middle of the image. At every time step, the robot takes an action that moves it towards a target pose, with some additive Gaussian noise (σ = 0.2). The target pose is sampled according to the same procedure as the initial pose, and it is sampled once at the beginning of each trajectory.
We try the fully and locally connected dynamics for pixel intensities to better understand the performance trade-offs when assuming locally connected dynamics. We do not use the latter for the semantic features since they are too high-dimensional for the dynamics model to fit in memory. The dynamics models were trained with ADAM using 10000 iterations, a batch size of 32, a learning rate of 0.001, and momentums of 0.9 and 0.999, and a weight decay of 0.0005.
C.3 LEARNING WEIGHTING OF FEATURE DYNAMICS WITH REINFORCEMENT LEARNING
We use CEM, TRPO and FQI to learn the feature weighting and report the performance of the learned policies in Table 2. We use the cost function described in Appendix B, a discount factor of γ = 0.9, and trajectories of up to 100 steps. All the algorithms used initial weights of w = 1 and λ = 1, and a Gaussian exploration policy with the current policy as the mean and a fixed standard deviation σexploration = 0.2.
For the case of unweighted features, we use CEM to optimize for a single weight w and for the weights λ. For the case of weighted features, we use CEM to optimize for the full space of parameters, but we only do that for the pixel feature dynamics since CEM does not scale for highdimensional problems, which is the case for all the VGG features. Each iteration of CEM performs a certain number of noisy evaluations and selects the top 20% for the elite set. The number of noisy evaluations per iteration was 3 times the number of parameters being optimized. Each noisy evalua-
Feature Dynamics Observations from Test Executions Cost
tion used the average sum of costs of 10 trajectory rollouts as its evaluation metric. The parameters of the last iteration were used for the final policy. The policies with unweighted features dynamics and the policies with pixel features dynamics were trained for 10 and 25 iterations, respectively.
We use TRPO to optimize for the full space of parameters for each of the feature dynamics we consider in this work. We use a Gaussian policy, where the mean is the servoing policy of Equation (3) and the standard deviation is fixed to σexploration = 0.2 (i.e. we do not learn the standard deviation). Since the parameters are constrained to be non-negative, we parametrize the TRPO policies with √ w and √ λ. We use a Gaussian baseline, where the mean is a 5-layer CNN, consisting of 2 convolutional and 3 fully connected layers, and a standard deviation that is initialized to 1. The convolutional layers use 16 filters (4 × 4, stride 2) each, the first 2 fully-connected layers use 32 hidden units each, and all the layers except for the last one use ReLU activations. The input of the baseline network are the features (either pixel intensities or VGG features) corresponding to the feature dynamics being used. The parameters of the last iteration were used for the final policy. The policies are trained with TRPO for 50 iterations, a batch size of 4000 samples per iteration, and a step size of 0.01.
We use our proposed FQI algorithm to optimize for the weights w,λ, and surpass the other methods in terms of performance on test executions, sample efficiency, and overall computation efficiency7. The updates of the inner iteration of our algorithm are computationally efficient; since the data is fixed for a given sampling iteration, we can precompute φ (st,ut) and certain terms of φ (st+1, ·). The parameters that achieved the best performance on 10 validation trajectories were used for the final policy. The policies are trained with FQI for S = 2 sampling iterations, a batch size of 10 trajectories per sampling iteration, K = 10 inner iterations per sampling iteration, and a regularization coefficient of ν = 0.1. We found that regularization of the parameters was important for the algorithm to converge. We show sample trajectories of the resulting policies in Table 3.
The FQI algorithm often achieved most of its performance gain after the first iteration. We ran additional sampling iterations of FQI to see if the policies improved further. For each iteration, we evaluated the performance of the policies on 10 validation trajectories. We did the same for the policies trained with TRPO, and we compare the learning curves of both methods in Figure 7.
7Our policy based on conv4 3 features takes around 650 s to run K = 10 iterations of FQI for a given batch size of 10 training trajectories.
C.4 LEARNING END-TO-END SERVOING POLICIES WITH TRPO
We use TRPO to train end-to-end servoing policies for various observation modalities and report the performance of the learned policies in Table 4. The policies are trained with the set of training cars, and tested on both this set and on the set of novel cars. The observation modalities that we consider are ground truth car positions (relative to the quadcopter), images of pixel intensities from the quadcopter’s camera, and VGG features extracted from those images. Unlike our method and the other experiments, no feature dynamics are explicitly learned for these experiments.
We use a Gaussian policy, where the mean is either a multi-layer perceptron (MLP) or a convolutional neural net (CNN), and the standard deviation is initialized to 1. We also use a Gaussian baseline, which is parametrized just as the corresponding Gaussian policy (but no parameters are shared between the policy and the baseline). For the policy that takes in car positions, the mean is parametrized as a 3-layer MLP, with tanh non-linearities except for the output layer; the first 2 fully connected layers use 32 hidden units each. For the other policies, each of their means is parametrized as a 5-layer CNN, consisting of 2 convolutional and 3 fully-connected layers, with ReLU non-linearities except for the output layer; the convolutional layers use 16 filters (4×4, stride 2) each and the first 2 fully-connected layers use 32 hidden units each.
The CNN policies would often not converge for several randomly initialized parameters. Thus, at the beginning of training, we tried multiple random seeds until we got a policy that achieved a relatively low cost on validation trajectories, and used the best initialization for training. The MLP policy did not have this problem, so we did not have to try multiple random initializations for it. All the policies are trained with a batch size of 4000 samples, 500 iterations, and a step size of 0.01. The parameters of the last iteration were used for the final policy.
C.5 CLASSICAL IMAGE-BASED VISUAL SERVOING
Traditional visual servoing techniques (Feddema & Mitchell, 1989; Weiss et al., 1987) use the image-plane coordinates of a set of points for control. For comparison to our method, we evaluate the servoing performance of feature points derived from bounding boxes and keypoints derived from hand-engineered features, and report the costs of test executions on Table 5.
We use bounding boxes from the C-COT tracker (Danelljan et al., 2016) (the current state-of-the-art visual tracker) and ground truth bounding boxes from the simulator. The latter is defined as the box that tightly fits around the visible portions of the car. We provide the ground truth bounding box of the first frame to the C-COT tracker to indicate that we want to track the car. We use the four corners of the box as the feature points for servoing to take into account the position and scale of the car in image coordinates.
We provide the ground truth depth values of the feature points for the interaction matrices. In classical image-based visual servoing, the control law involves the interaction matrix (also known as feature Jacobian), which is the Jacobian of the points in image space with respect to the camera’s control (see Chaumette & Hutchinson (2006) for details). The analytical feature Jacobian used in IBVS assumes that the target points are static in the world frame. This is not true for a moving car, so we consider a variant where the feature Jacobian incorporates the ground truth dynamics of the car. This amounts to adding a non-constant translation bias to the output of the dynamics function, where the translation is the displacement due to the car’s movement of the 3-dimensional point in the camera’s reference frame. Note that this is still not exactly equivalent to having the car being static since the roads have different slopes but the pitch and roll of the quadcopter is constrained to be fixed.
For the hand-crafted features, we consider SIFT (Lowe, 2004), SURF (Bay et al., 2006) and ORB (Rublee et al., 2011) keypoints. We filter out the keypoints of the first frame that does not belong to the car and use these as the target keypoints. However, we use all the keypoints for the subsequent observations.
The servoing policies based on bounding box features achieve low cost, and even lower ones if ground truth car dynamics is used. However, servoing with respect to hand-crafted feature points is significantly worse than the other methods. This is, in part, because the feature extraction and matching process introduces compounding errors. Similar results were found by Collewet & Marchand (2011), who proposed photometric visual servoing (i.e. servoing with respect to pixel intensities) and showed that it outperforms, by an order of magnitude, classical visual servoing that uses SURF features.
C.6 CLASSICAL POSITION-BASED VISUAL SERVOING
Position-based visual servoing (PBVS) techniques use poses of a target object for control (see Chaumette & Hutchinson (2006) for details). We evaluate the servoing performance of a few variants, and report the costs of test executions on Table 6.
Similar to our IBVS experiments, we consider a variant that uses the car pose of the next time step as a way to incorporate the ground truth car dynamics into the interaction matrix. Since the cost function is invariant to the orientation of the car, we also consider a variant where the policy only minimizes the translational part of the pose error.
These servoing policies, which use ground truth car poses, outperforms all the other policies based on images. In addition, the performance is more than two orders of magnitude better if ground truth car dynamics is used.
1) Summary
This paper proposes to tackle visual servoing (specifically target following) using spatial feature maps from convolutional networks pre-trained on general image classification tasks. The authors combine bilinear models of one-step dynamics of visual feature maps at multiple scales with a reinforcement learning algorithm to learn a servoing policy. This policy is learned by minimizing a regularized weighted average of distances to features predicted by the aforementioned model of visual dynamics.
2) Contributions
+ Controlled experiments in simulation quantifying the usefulness of pre-trained deep features for visual servoing.
+ Clear performance benefits with respect to many sensible baselines, including ones using ground truth bounding boxes.
+ Principled learning of multi-scale visual feature weights with an efficient trust-region fitted Q-iteration algorithm to handle the problem of distractors.
+ Good sample efficiency thanks to the choice of Q-function approximator and the model-based one-step visual feature dynamics.
+ Open source virtual city environment to benchmark visual servoing.
3) Suggestions for improvement
- More complex benchmark:
Although the environment is not just a toy synthetic one, the experiments would benefit greatly from more complex visual conditions (clutter, distractors, appearance and motion variety, environment richness and diversity, etc). At least, the realism and diversity of object appearances could be vastly improved by using a larger number of 3D car models, including more realistic and diverse ones that can be obtained from Google SketchUp for instance, and populating the environment with more distractor cars (in traffic or parked). This is important as the main desired quality of the approach is robustness to visual variations.
- End-to-end and representation learning:
Although the improvements are already significant in the current synthetic experiments, it would be interesting to measure the impact of end-to-end training (i.e. also fine-tuning the convnet), as it is possibly needed for better generalization in more challenging visual conditions. It would also allow to measure the benefit of deep representation learning for visual servoing, which would be relevant to ICLR (there is no representation learning so far, although the method can be straightforwardly adapted as the authors mention briefly).
- Reproducibility:
The formalism and algorithms are clearly explained, but there is a slightly overwhelming mass of practical tricks and implementation details described with varying levels of details throughout the paper and appendix. Grouping, simplifying, or reorganizing the exposition of these implementation details would help, but a better way would probably consist in only summarizing the most important ones in section and link to an open source implementation of the method for completeness.
- Typos:
p.2: "learning is a relative[ly] recent addition"
p.2: "be applied [to] directly learn"
4) Conclusion
In spite of the aforementioned limits of the experiments, this paper is interesting and solid, in part thanks to the excellent reply to the pre-review questions and the subsequent improved revision. This leads me to believe the authors are more than capable of following to a significant extent the aforementioned suggestions for improvement, thus leading to an even better paper.