Complex articulated object tracking

Electronic Letters on Computer Vision and Image Analysis 5(3):21-31, 2005 Complex articulated object tracking Andrew I. Comport, ´ Eric Marchand and Franc¸ois Chaumette IRISA - INRIA Rennes, Campus de Beaulieu, 35042 Rennes, France Received 20 December 2004; accepted 9 March 2005 Abstract In this paper new results are presented for tracking complex multi-body objects. The theoretical framework is based on robotics techniques and uses an a-priori model of the object including a general mechanical link description. A new kinematic-set formulation takes into account that articulated degrees of freedom are directly observable from the camera and therefore their estimation does not need to pass via a kinematicchain back to the root. By doing this the tracking techniques are efficient and precise leading to real-time performance and accurate measurements. The system is locally based upon an accurate modeling of a distance criteria. A general method is given for defining any type of mechanical link and experimental results show prismatic, rotational and helical type links. A statistical M-estimation technique is applied to improve robustness. A monocular camera system was used as a real-time sensor to verify the theory. Key Words: Computer Vision, Image Registration, Non-rigid Motion, Multi-body Systems, Computer Vision, 3D Tracking, Articulated Objects, Kinematic Sets, Visual Servoing, Model-Based, Real-time. 1 Introduction Previously, non-rigid motion has been classed into three categories describing different levels of constraints on the movement of a body: articulated, elastic and fluid [1]. In this paper the first class of non-rigid motion is considered and a link is made with the remaining classes. An ”articulated” object is defined as a multibody system composed of at least two rigid components and at most six independent degrees of freedom between any two components. With articulated motion, a non-rigid but constrained dependence exists between the components of an object. Previous methods have attempted to describe articulated motion either with or without an a-priori model of the object. In this study a 3D model is used due to greater robustness and efficient computation. Knowing the object in advance helps to predict hidden movement, which is particularly interesting in the case of non-rigid motion because there is an increased amount of self-occlusion. Knowing the model also allows an analytic relation for the system dynamics to be more precisely derived. 1.0.1 State of the Art In general, the methods which have been proposed in the past for articulated object tracking rely on a good rigid tracking method. In computer vision the geometric primitives considered for tracking have been numerous, however, amongst them distance based features have shown to be efficient and robust [11, 6, 14, 2]. Another important issue is the 2D-3D registration problem. Purely geometric (eg, [5]), or numerical and iterative [4] Correspondence to: <[email protected]> Recommended for acceptance by <Perales F., Draper B.> ELCVIA ISSN:1577-5097 Published by Computer Vision Center / Universitat Aut`onoma de Barcelona, Barcelona, Spain A.I. Comport et al. / Electronic Letters on Computer Vision and Image Analysis 5(3):21-31, 2005 22 Articular pose Final pose component1 Pose component1 Component 2 Component 1 Component 3 Articular pose Camera at tCamera at t−1                   Figure 1: Kinematic chain method: The pose of an articulated object is determined via a kinematic chain of rigid bodies extending to sub components. The poses rdescribing the state of the system are intuitive. approaches may be considered. Linear approaches use a least-squares method to estimate the pose and are considered to be more suitable for initialization procedures. Full-scale non-linear optimization techniques (e.g., [11, 13, 6, 2]) consists of minimizing the error between the observation and the forward-projection of the model. In this case, minimization is handled using numerical iterative algorithms such as Newton-Raphson or Levenberg-Marquardt. The main advantage of these approaches are their accuracy. The main drawback is that they may be subject to local minima and, worse, divergence. This approach is better suited to maintaining an already initialized estimation. Within this context it is possible to envisage different ways to model the pose of an articulated object. The first method for tracking articulated objects using kinematic chains (see Figure 1.0.1) appears in well known work by Lowe [12]. He demonstrates a classical method using partial derivatives. In his paper the kinematic chain of articulations is represented as tree structure of internal rotation and translation parameters and the model points are stored in the leaves of this tree.The position and partial derivatives of each point in cameracentered coordinates is determined by the transformations along the path back to the root. Recently, more complex features have been used for non-rigid object tracking in [16]. They make use of deformable super-quadric models combined with a kinematic chain approach. However, real-time performance is traded-off for more complex models. Furthermore, this method requires multiple viewpoints in order to minimize the system of equations. As Lowe points out, the tendencies in computer graphics have been toward local approximations via polyhedral models. Ruff and Horaud [17] give another kinematic-chain style method for the estimation of articulated motion with an un-calibrated stereo rig. They introduce the notion of projective kinematics which allows rigid and articulated motions to be represented within the transformation group of projective space. The authors link the inherent projective motions to the Lie-group structure of the displacement group. Minimization is performed in projective space making the parameters invariant to camera calibration. A second approach has been proposed by Drummond and Cippola [6] which treats articulated objects as groups of rigid components with constraints between them directly in camera coordinates (see Figure 1.0.1). It appears that the full pose of each rigid component is initially computed independently requiring the estimation of a redundant number of parameters. Lagrange multipliers are then used to constrain these parameters according to simple link definitions. This method uses Lie Algebra to project the measurement vector (distances) onto the subspace defined by the Euclidean transformation group (kinematic screw). They also implement M-estimation to improve robustness. 23A.I. Comport et al. / Electronic Letters on Computer Vision and Image Analysis 5(3):21-31, 2005 6 Parameters 6 Parameters 6 Parameters Lagrange multiplier constraint n parameters Camera at t−1 Camera at t      "! $# &% ')(    +* Figure 2: Lagrange Multiplier method: The pose between the camera and each component of the object is calculated individually in a first step. Constraints are then applied between the full pose of each component via Lagrange multipliers in a second step. 1.0.2 Contribution A new model is proposed in this paper which is based on the observation that within a vision system one has direct access with a camera to the parameters of an articulated object. Thus, unlike traditional techniques using robotics based approaches, there is no need to sum partial derivatives along a kinematic chain back to the root. As will be shown, the joint reference frame plays an important role in modeling articulated objects. The method presented in this paper also integrates a mechanical link formulation for simple definition of articulations. It is important to correctly model the behavior of the system to obtain maximum decoupling of joint parameters and therefore interesting minimization properties. In this paper a kinematic set approach is proposed. With articulated motion, unlike the case of rigid motion, the subsets of movement which may be attributed to either the object or camera are not unique. A novel subset approach is used whereby the minimization is carried out on decoupled subsets of parameters by defining subspace projectors from the joint definitions. This allows the error seen in the image to be partially decoupled from the velocities of the object by determining the independent sets of velocities present in object space. The principal advantages of this approach are that it: •is more efficient in terms of computation than previous methods. •eliminates the propagation of errors between free parameters. •models more closely the real behavior of the system than a camera frame based approach. In the remainder of this paper, Section 2 presents the principle of the approach. In Section 3 articulated object motion and velocity are defined. In Section 4 a non-linear control law is derived for tracking articulated objects. In Section 5, several experimental results are given for different virtual links. A.I. Comport et al. / Electronic Letters on Computer Vision and Image Analysis 5(3):21-31, 2005 24 2 Overview and Motivations The objective of the proposed approach is to maintain an estimate of a set of minimal parameters describing the configuration of an articulated object in SE(n). This set of parameters are defined by a vector of n parameters q∈Rn. This vector is composed of subsets which fully describe the velocity of each component. In order to maintain an estimate of q, the underlying idea is to minimize a non-linear system of equations so that the projected contour of the object model in the image is aligned with the actual position of the contours in the image. This can be seen as the dual problem of visual servoing whereby minimizing the parameters corresponds to moving an arm-to-eye robot so as to observe the arm at a given position in the image (note that an object is not necessarily fixed to the ground). This duality, known as Virtual Visual Servoing has been explained in depth in previous papers [2, 15]. To perform the alignment, an error ∆is defined in the image between the projected features s(q)of the model and their corresponding features in the image sd(desired features). The features of each component are projected using their associated camera poses crF1(q)and crF2(q)where each component’s camera pose is composed of a subset of object parameters q. In this paper distance features are used. This error is therefore defined as: ∆ = s(q)−sd=pr(q,oS)−sd,(1) where oSare the 3D coordinates of the sensor features in the object frame of reference. pr(q,oS)is the camera projection model according to the object parameters q. The parameters of the object are initially needed and they are computed using the algorithm of Dementhon and Davis [4]. This algorithm is used to calculate the component’s poses in the camera frame and they are calculated separately. The parameters are projected into object space and variables in common between the components are averaged so that initialization errors are minimal. In order to render the minimization of these errors more robust they are minimized using a robust approach based on M-estimation techniques. ∆R=ρs(q)−sd,(2) where ρ(u)is a robust function [9] that grows sub-quadratically and is monotonically nondecreasing with increasing |u|. In this article Tukey’s function is used because it allows complete rejection of outliers. This is integrated into an iteratively re-weighted least squares(IRLS) minimization procedure so as to render those errors at the extremities of the distribution less likely. 3 Modeling Articulated motion is defined as Euclidean transformations which preserve subsets of distances and orientation of object features. The modeling of object motion is based on rigid body differential geometry. The set of rigid-body positions and orientations belongs to a Lie group, SE(3) (Special Euclidean group). These vectors are known as screws. The tangent space is the vector space of all velocities and belongs to the Lie algebra, se(3). This is the algebra of twists which is also inherent in the study of non-rigid motion. An articulated object, for example, must have a velocity contained in se(3), however, joint movement can be considered by sub-algebras of se(3). The basic relation can be written which relates the movement of a sensor feature ˙ sto the movement of the object parameters: ˙s =LsA ˙q (3) where •Lsis called the feature Jacobian [10] or interaction matrix [7] between the camera and the sensor features s. •Ais an Articulation matrix describing the differential relation between components. 25A.I. Comport et al. / Electronic Letters on Computer Vision and Image Analysis 5(3):21-31, 2005 Object at t−1 Object at t Other object parameters Camera stationary                   !  !" Figure 3: Kinematic set method: The joint parameters are minimized in object space and kinematic set are used to decouple the system. Decoupling occurs at the intersection of parameter sets. A,B,Care the sets of velocity parameters corresponding to each rigid component. ˙q is a vector of generalized velocity parameters. The poses rcorrespond to the joint poses in 3D. •LsAbeing the Jacobian between the sensor and the entire object. The Articulation matrix is the central issue in this paper. It corresponds to the mapping: v=A ˙q (4) where vis a vector of ’stacked’ 6dimensional twists each corresponding to the full motion in se(3) of each component. The subsets of parameters which make up the object parameters are illustrated by a Venn diagram in Figure 3. In order that these sets can be obtained independently it is necessary to decouple their interaction. The only case where this occurs is in the joint frame of reference. Thus the following section considers the definition of a joint. 3.1 Mechanical Joint Concept A mechanical joint is fully defined by a matrix, vector pair which links two components. It is composed of a constraint matrix S⊥which defines the type of the link and a pose vector rdefining the position of the articulation. Thus the articulation matrix is: Al(S⊥ l,rl),(5) where S⊥ lcorresponds to the configuration of joint land ris a parameter vector describing the location of joint l. The following two subsections explain the definition of these parameters. A.I. Comport et al. / Electronic Letters on Computer Vision and Image Analysis 5(3):21-31, 2005 26 3.2 Joint Configuration - Sl A joint configuration is fully defined by: S⊥ l=   s⊥ 1,1. . . s⊥ 1,c . . .... s⊥ 6,1s⊥ 6,c   ,(6) The holonomic constraint matrix, S⊥, is defined such that each column vector defines one free degree of freedom at the corresponding link. The number of non-zero columns of S⊥is referred to as the class cof the link. The rows of a column define the type of the link by defining which combination of translations and rotations are permitted as well as their proportions. In the experiments considered in Section 5 two different types of class 1 links are considered: A rotational link around the x axis: S⊥= (0,0,0,1,0,0),(7) A helical link around and along the z axis: S⊥= (0,0, a, 0,0,1),(8) where the value of ’a’ relates the translation along the zaxis to a one rotation around the zaxis. The set of velocities that a first component can undertake which leaves a second component invariant is defined by S⊥⊂se(3). This is the orthogonal compliment of the sub-space S⊂se(3) which constitutes the velocities which are in common between two components. Since a component, that is linked to another, is composed of these two subspaces it is possible to extract these subspaces by defining standard bases for the kernel and the image. The kernel is chosen to be S⊥so that the image is given by (with abuse of notation): Sl=Ker((S⊥ l)T),(9) The matrix Sland its orthogonal compliment S⊥ lcan be used to project the kinematic twist (velocities) onto two orthogonal subspaces (For more than 1 joint it is necessary to project onto a common vector basis). Thus a subspace projection matrix is given as: Pl=SlS+ l, P⊥ l=S⊥ lS⊥+ l=I6−Pl,(10) where S+= (STS)−1STis the pseudo-inverse of S. This ensures that the resulting projected velocities are defined according to a common basis defined by the parameters of the pose vector in equation (6). This then allows the twist transformations, given in the following section, to be applied to these quantities. 3.3 Joint Location - rl A joint location is fully defined by a pose vector: rl=Fcrl= (tx, ty, tz, θx, θy, θz),(11) where Fcindicates the camera frame and lrepresents the joint frame. A joint configuration is only valid in the joint reference frame. A kinematic twist transformation matrix is used to obtain the velocity of the joint frame w.r.t its previous position. The Lie algebra provides the transformation of vector quantities as V(r). This is a kinematic twist transformation from frame ato frame bgiven as: aVb=aRb[atb]a ×Rb 03aRb,(12) 27A.I. Comport et al. / Electronic Letters on Computer Vision and Image Analysis 5(3):21-31, 2005 where aRbis a rotation matrix between frames and atba translation vector between frames which are obtained from arb.[t]xis the skew symmetric matrix related to t. The projector defined in (10) is applied in the joint reference frame. It is possible to choose the object frame as a common reference frame as in [3]. In this paper the camera frame is chosen as the common reference frame so that a generic subspace projection operators Jland J⊥ lcan be defined as: Jl=Im(cVlPllVc), J⊥ l=Ker(J) = Im(cVlP⊥ llVc),(13) where Im represents the Image operator which reduces the column space to its mutually independent basis form. The first transformation Vmaps the velocities to the joint frame land the second re-maps back to the camera reference frame. 3.4 Articulation Matrix Using the previous joint definition is is possible to define the Articulation matrix according to equation (4) and taking into account the joint subspaces given by equation (13). The derivation of the Articulation matrix corresponds to: A=   ∂r1 ∂q . . . ∂rm ∂q   ,(14) where mis the number of components. For an object with two components and one jointand using the orthogonal subspace projectors given in equation (13), Ais given by: A= ∂r1 ∂q∩ ∂r1 ∂q10 ∂r2 ∂q∩0∂r2 ∂q2!=J1J⊥ 10 J10 J⊥ 1,(15) where q∩,q1,q2are vectors representing the sets of intersecting velocities and each components free parameters respectively. These sets are easily identified when referring to Figure 3. Given dim(J1) = 6 −cand dim(J⊥ 1) = c, the mapping Ais indeed dimension 12 ×(6 + c), remembering that cis the class of the link. The derivation of objects with more than one joint follows in a similar manner and is left to the reader. It is important to note that this method introduces decoupling of the minimization problem. This is apparent in equation (15) where extra zeros appear in the Jacobian compared to the traditional case of a kinematic chain. Indeed, in the particular case of two components and one articulation a kinematic chain has only one zero. 4 Registration In this section a new tracking control law is derived. The aim of the control scheme is to minimize the objective function given in equation (2). Thus, the error function is given as:    e1 . . . em   =D   s1(q)−sd1 . . . sm(q)−sdm   ,(16) where qis a vector composed of the minimal set of velocities corresponding to the object’s motion and each eicorresponds to an error vector for component i.Dis a diagonal weighting matrix corresponding to the A.I. Comport et al. / Electronic Letters on Computer Vision and Image Analysis 5(3):21-31, 2005 28 likelihood of a particular error within the robust distribution: D=   D10 ... 0Dm   , where each matrix Diis a diagonal matrix corresponding to a component which has weights wjalong the diagonal. These weights correspond to the uncertainty of the measured visual feature j. The computation of the weights are described in [2]. If Dwere constant, the derivative of equation (16) would be given by:    ˙ e1 . . . ˙ ei   =   ∂e1 ∂s1 ∂s1 ∂r1 ∂r1 ∂q . . . ∂em ∂sm ∂sm ∂rm ∂rm ∂q   ˙q =DLsA ˙q (17) where ˙q is the minimal velocity vector, Ais the articulation matrix describing the mapping in equation (4) and Lsis the ’stacked’ interaction matrix as in equation (3) given as: Ls=   ∂s1 ∂r1 . . . ∂sm ∂rm   =   Ls106 ... 06Lsm   ,(18) If an exponential decrease of the error eis specified: ˙ e=−λe,(19) where λis a positive scalar, the following control law is obtained by combining equation (19) and equation (17): ˙q =−λ(b Dc Lsb A)+b D(s(q)−sd),(20) where c Lsis a model or an approximation of the real matrix Ls.b Da chosen model for Dand b Adepends on the previous pose estimation. For the example of one joint given in equation (15), the sets of velocities to be estimated are: ˙q =˙q∩,˙q1,˙q2,(21) Once these velocities are obtained they can be related back to the camera frame as in equation (4): cv1 cv2=A  ˙q∩ ˙q1 ˙q2 (22) 5 Results In this section three experiments are presented for tracking of articulated objects in real sequences. Both camera and object motion as well as articulated motion have been introduced into each experiment. The complex task of implementing this algorithm was a major part of the work. Indeed this required correct modeling of features of type distance to lines, correct modeling of feature sets and correct implementation of the interaction between these feature sets represented as a graph of feature sets. 29A.I. Comport et al. / Electronic Letters on Computer Vision and Image Analysis 5(3):21-31, 2005 The rigid tracking method used here is based on a monocular vision system. Local tracking is performed via a 1D oriented gradient search to the normal of parametric contours at a specified sampling distance. This 1D search provides real-time performance. Local tracking provides a redundant group of distance to contour based features which are used together in order to calculate the global pose of the object. The use of redundant measures allows the elimination of noise and leads to high estimation precision. These local measures form an objective function which is minimized via a non-linear minimization procedure using virtual visual servoing(VVS) [2]. These previous results demonstrate a general method for deriving interaction matrices for any type of distance to contour and also show the robustness of this approach with respect to occlusion and background clutter. The basic implementation of the algorithm gives the following pseudo-code: 1. Obtain initial pose. 2. Acquire new image and project the model onto the image. 3. Search for corresponding points normal to the projected contours. 4. Determine the error ein the image. 5. Calculate (b Db Hb A). 6. Determine set velocities as in equation (20) and then component positions. 7. Repeat to 4 until the error converges. 8. Update the pose parameters and repeat to 3. 5.1 Helical Link This first experiment, reported in Figure 4 was carried out for class one link with helical movement simultaneously along and around the zaxis. The constraint vector was defined as in equation (8) and the object frame was chosen to coincide with the joint frame. Note that the constraint vector was defined by taking into consideration that for 10 ×2πrotations of the screw it translated 4.5cm along the zaxis. Figure 4: Helical movement of a screw whilst the screw and the platform are simultaneously in movement.In this and all the following figures the reference frames for each component are shown as well as a projection of the CAD model onto the image. The axes of the frames are drawn in yellow, blue and red. The contour of the object is shown in blue. The points found to the normal of the contour are in red and the points rejected by the M-estimator are shown in green. Tracking of this object displayed real time efficiency with the main loop computation taking on average 25ms per image. It should be noted that tracking of the screw alone as a rigid object fails completely due to the limited contour information and difficult self occulsions. When tracked simultaneously with the plate as an articulated object the tracking of the screw is also based on the measurements of the plate making the tracking possible. M-estimation was carried out separately for each component.