3-D Vision for Navigation and Grasping

Abstract

In this chapter, we describe algorithms for three-dimensional (GlossaryTerm

3-D

) vision that help robots accomplish navigation and grasping. To model cameras, we start with the basics of perspective projection and distortion due to lenses. This projection from a 3-D world to a two-dimensional (GlossaryTerm

2-D

) image can be inverted only by using information from the world or multiple 2-D views. If we know the 3-D model of an object or the location of 3-D landmarks, we can solve the pose estimation problem from one view. When two views are available, we can compute the 3-D motion and triangulate to reconstruct the world up to a scale factor. When multiple views are given either as sparse viewpoints or a continuous incoming video, then the robot path can be computer and point tracks can yield a sparse 3-D representation of the world. In order to grasp objects, we can estimate 3-D pose of the end effector or 3-D coordinates of the graspable points on the object.

These keywords were added by machine and not by the authors. This process is experimental and the keywords may be updated as the learning algorithm improves.

With the rapid progress and cost reduction in digital imaging, cameras became the standard and probably the cheapest sensor on a robot. Unlike positioning (global positioning system – GlossaryTerm

GPS

), inertial (GlossaryTerm

IMU

), and distance sensors (sonar, laser, infrared) cameras produce the highest bandwidth of data. Exploiting information useful for a robot from such a bit stream is less explicit than in case of GPS or a laser scanner but semantically richer. In the years since the first edition of the handbook, we had significant advances in hardware and algorithms. RGB-D sensors like the Primesense Kinect enabled a new generation of full model reconstruction systems [32.1] with an arbitrary camera motion. Google’s project Tango [32.2] established the state of the art in visual odometry using the latest fusion methods between visual and inertial data ([32.3] and ). 3-D modeling became a commodity software (see, for example, 123D Catch App from Autodesk) and the widely used open source Bundler ([32.4] ) has been possible by advances in wide baseline matching and bundle adjustment. Methods for wide baseline matching have been proposed for several variations of pose and structure from motion [32.5]. Last, the problem of local minima for nonminimal overconstrained solvers has been addressed by a group of method using Branch and Bound global optimization of a sum of fractions subject to convex constraints [32.6] or an $L_{\mathrm{\infty }}$-norm of the error function [32.7].

Let us consider the two main robot perception domains: navigation and grasping. Assume for example the scenario that a robot vehicle is given the task of going from place A to place B given as instruction only intermediate visual landmarks and/or GPS waypoints. The robot starts at A and has to decide where is a drivable path. Such a decision can be accomplished through the detection of obstacles from at least two images by estimating a depth or occupancy map with a stereo algorithm. While driving, the robot wants to estimate its trajectory which can be accomplished with a matching and structure from motion algorithm. The result of the trajectory can be used to build a lay out of the environment through dens matching and triangulation which in turn can be used as a reference for a subsequent pose estimation. At each time instance the robot has to parse the surrounding environment for risks like pedestrians, or for objects it is searching for like a trash-can. It has to become aware of loop closing or a reentry if the robot has been kidnaped or blind for a while. This can be accomplished through object and scene recognition yielding the what and where of objects around the robot. In an extreme scenario, a vehicle can be left to explore a city and build a semantic 3-D map as well as a trajectory of all places it visited, the ultimate visual simultaneous localization and semantic mapping problem. In the case of grasping, the robot detects an object given a learnt representation, and subsequently, it has to estimate the pose of the object and in some cases its shape by triangulation. When a camera is not mounted on an end-effector, the absolute orientation between the hand the object has to be found.

In the next section we will present the geometric foundations for 3-D vision and in the last section we describe approaches for grasping.

1 Geometric Vision

Let us start by introducing the projection of the world to an image plane. Assume that a point in the world $(X,Y,Z)$ has coordinates $(X_{\mathrm{c}i},Y_{\mathrm{c}i},Z_{\mathrm{c}i})$ with respect to the coordinate system of a camera c_i related to each other by the following transformation

$$\left(\begin{array}{c}{X}_{\mathrm{c}i}\\ Y_{\mathrm{c}i}\\ Z_{\mathrm{c}i}\end{array}\right)={\mathbf{R}}_i\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)+{\mathit{T}}_i,$$

(32.1)

where R_i is a rotation matrix whose columns are the world axes with respect to the camera. The translation vector T_i is starting from the origin of the camera and ending at the origin of the world coordinate system. The rotation matrix is orthogonal ${\mathbf{R}}^{\mathrm{T}}\mathbf{R}=\mathbf{1}$ with determinant one. We assume that the center of projection is the origin of the coordinate system and that the optical axis is the $Z_{\mathrm{c}i}$ axis of the camera. If we assume that the image plane is the plane $Z_{\mathrm{c}i}=1$ then the image coordinates $(x_i,y_i)$ read

$$x_i=\frac{X_{\mathrm{c}i}}{Z_{\mathrm{c}i}},y_i=\frac{Y_{\mathrm{c}i}}{Z_{\mathrm{c}i}}.$$

(32.2)

In practice, what we measure are the pixel coordinates $(u_i,v_i)$ in the image which are related to image coordinates $(x_i,y_i)$ with the affine transformation

$$u_i=f\alpha x_i+\beta y_i+c_u,v_i=f{y}_i+c_v,$$

(32.3)

where f is the distance of the image plane to the projection center measured in pixels. It is also called focal length, because they are considered approximately equal. The aspect ratio α is a scaling induced by nonsquare sensor cells or different sampling rates horizontally and vertically. The skew factor β accounts for a shearing induced by a nonperfectly frontal image plane. The image center $c_u,c_v$ is the point of intersection of the image plane with the optical axis called the image center. These five parameters are called intrinsic parameters and the process of recovering them is called intrinsic calibration. Upon recovering them we can talk about a calibrated system and we can work with the image coordinates $(x_i,y_i)$ instead of the pixel coordinates $(u_i,v_i)$. In many vision systems in particular on mobile robots, wide-angle lenses introduce a radial distortion around the image center which can be modelled polynomially

$$\begin{array}{rl}{x}_i^{\text{dist}}& =x_i(1+k_{1}r+k_2{r}^2+k_3{r}^3+\mathrm{\dots })\\ y_i^{\text{dist}}& =y_i(1+k_{1}r+k_2{r}^2+k_3{r}^3+\mathrm{\dots })\\ \text{where}{r}^2& =x_i^2+y_i^2,\end{array}$$

where we temporarily assumed that the image center is at (0,0). The image coordinates $(x_i,y_i)$ in (32.3) have to be replaced with the distorted coordinates $(x^{\text{dist}},y^{\text{dist}})$.

1.1 Calibration

Recovering the intrinsic parameters when we can make multiple views of a reference pattern like a checker board without variation of the intrinsic parameters has become a standard procedure using tools like the MATLAB calibration toolbox or Zhang’s OpenCV calibration function [32.8]. When intrinsics like the focal length vary during operation and viewing reference patterns is not practically feasible, we rely on the state of the art method by Pollefeys et al. [32.10, 32.9]. When all intrinsic are unknown on the Kruppa equations and several stratified self-calibration approaches [32.11, 32.12] which require at least three views. Apart radial distortion, the projection relations shown above can be summarized in matrix form. By denoting

$${\mathit{u}}_i=(u_i,v_i,1)$$

and

$$\mathit{X}=(X,Y,Z,1)$$

we obtain

$${\lambda }_i{\mathit{u}}_i={\mathbf{K}}_i\left({\mathbf{R}}_i,{\mathit{T}}_i\right)\mathit{X}=\mathbf{P}\mathit{X},$$

(32.4)

where ${\lambda }_i=Z_{\mathrm{c}i}$ is the depth of point X in camera coordinates and P is the 3 × 4 projection matrix. The depth λ_i which can be eliminated to obtain two equations relating the world to the pixel coordinates.

1.2 Pose Estimation or PnP

When we have landmarks in the world with known positions X and we can measure their projections, the problem of recovering the unknown rotation and translation in the calibrated case is called pose estimation or the Prespective-n-Point problem (GlossaryTerm

PnP

). Of course, it presumes the identification of the world points in the image. In robotics, the pose estimation is a variant of the localization problem in a known environment. When grasping objects of known shape PnP yields the target pose for an end-effector module the grasping point positions. We assume that a camera is calibrated and that measurements of N points are given in world coordinates ${\mathit{X}}_{j=\mathrm{1..}N}$ and calibrated image coordinates ${\mathit{x}}_{j=\mathrm{1..}N}$. Let us assume two scene points and denote the known angle between their projections ${\mathit{x}}_1$ and ${\mathit{x}}_2$ as δ₁₂ (Fig. 32.1). Let us denote the squared distance $\parallel {\mathit{X}}_i-{\mathit{X}}_j{\parallel }^2$ with $d_{ij}^2$ and the lengths of X_j with $d_j^2$. Then cosine law reads

$$d_i^2+d_j^2-2d_i{d}_j\cos {\delta }_{ij}=d_{ij}^2.$$

(32.5)

If we can recover d_i and d_j the rest will be an absolute orientation problem

$${\mathrm{d}}_j{\mathit{x}}_j=\mathbf{R}{\mathit{X}}_j+\mathit{T}$$

(32.6)

to recover translation and rotation between camera and world coordinate system.

Fig. 32.1

Pose estimation problem: A camera seeing 3 points at unknown distances d₁, d₂, and d₃ with known angles between the rays and known point distances $d_{12},d_{13},d_{23}$

1.2.1 Minimal Solution

The cosine law has two unknowns d₁ and d₂ so with three points we should be able to solve for the pose estimation problem. Indeed, three points yield a system of three quadratic equations in three unknowns, so it will have a maximum of eight solutions.

We follow here the analysis of the classic solution in [32.13] and set $d_2=u{d}_1$ and $d_3=v{d}_1$ and solve all three equations for d₁

$$\begin{array}{rl}{d}_1^2& =\frac{d_{23}^2}{u^2+v^2-2uv\cos {\delta }_{23}},\\ d_1^2& =\frac{d_{13}^2}{1+v^2-2v\cos {\delta }_{13}},\\ d_1^2& =\frac{d_{12}^2}{u^2+1-2u\cos {\delta }_{12}},\end{array}$$

which is equivalent to two quadratic equations in u and v

$$d_{12}^2(1+v^2-2v\cos {\delta }_{13})=d_{13}^2(u^2+1-2u\cos {\delta }_{12}),$$

(32.7)

$$d_{13}^2(u^2+v^2-2uv\cos {\delta }_{23})=d_{23}^2(1+v^2-2v\cos {\delta }_{13}).$$

(32.8)

Solving (32.8) for u² and substituting in (32.7) allows solving E1 for u because u appears linearly. Substituting u back in (32.8) yields a quartic in v which can have as many as four real roots. For each v we obtain two roots for u through any of the quadratic equations yielding a maximum of eight solutions [32.13, 32.14]. Popular pose estimation algorithms are based either on an iterative method [32.15, 32.16] or linear versions using auxiliary unknowns of higher dimension [32.17, 32.18].

A more recent method [32.19] for n world points expresses 3-D points as the barycentric coordinates with respect to four virtual control points

$${\mathit{X}}_i=\sum _{j=1}^4{\alpha }_{ij}{\mathit{C}}_j,\text{where}\sum _{j=1}^4{\alpha }_{ij}=\mathrm{1\hspace{0.33em}.}$$

A rigid transformation to the camera coordinate system leaves the barycentric coordinates invariant and a perspective projection yields

$${\lambda }_i{\mathit{x}}_i=\sum _{j=1}^4{\alpha }_{ij}{\left(X_{\mathrm{c}i},Y_{\mathrm{c}i},Z_{\mathrm{c}i}\right)}^{\mathrm{T}}.$$

Eliminating λ_i yields two linear equations for each point

$$\begin{array}{rl}{\sum }_{j=1}^4{\alpha }_{ij}{C}_{x_{\mathrm{c}j}}& ={\alpha }_{ij}{x}_i{C}_{z_{\mathrm{c}j}}\\ {\sum }_{j=1}^4{\alpha }_{ij}{C}_{y_{\mathrm{c}j}}& ={\alpha }_{ij}{y}_i{C}_{z_{\mathrm{c}j}}\end{array}$$

with the coordinate triples of the control points in the camera frame being the 12 unknowns. This is a linear homogeneous system with the solution being the nullspace of a $2n\times 12$ matrix. The unknown control points are found up to a scale factor which is easily fixed because we know the inter point distances. The pose is found from absolute orientation between control points in the camera and the world frame. This yields a very efficient solution for $n\ge 6$ points but leaves you with the initial choice of the control points as a factor affecting the solution.

In case that $n\ge 4$ points lie on a plane we can compute the homography H between the world and the camera plane [32.8]. Assuming Z = 0 is the world plane the homography reads

$$\left(\begin{array}{c}u\\ v\\ w\end{array}\right)\approx {\underbrace{K\left(r_1,r_2,T\right)}}_H\left(\begin{array}{c}X\\ Y\\ W\end{array}\right),$$

where $r_{1,2}$ are the first two columns of the rotation matrix and ≈ denotes the projective equivalence, namely, for any two points p and ${\mathit{p}}^{\prime }$ in the projective plane $\mathit{p}\approx {\mathit{p}}^{\prime }$ iff $\mathit{p}=\lambda {\mathit{p}}^{\prime }$ for real $\lambda \ne 0$. Hence the first two columns of $K^{-1}H$

$$K^{-1}H=\left(h_1^{\prime },h_2^{\prime },h_3^{\prime }\right)$$

have to be orthogonal. We seek thus an orthogonal matrix R that is the closest to $\left(h_1^{\prime },h_2^{\prime },h_1^{\prime }\times h_2^{\prime }\right)$

$${\text{arg min}}_{R\in \text{SO}\left(3\right)}\parallel \mathbf{R}-\left(h_1^{\prime },h_2^{\prime },h_1^{\prime }\times h_2^{\prime }\right){\parallel }_F^2.$$

If the singular value decomposition (GlossaryTerm

SVD

) of

$$\left(h_1^{\prime },h_2^{\prime },h_1^{\prime }\times h_2^{\prime }\right)=US{V}^{\mathrm{T}},$$

then the solution is [32.20]

$$\mathbf{R}=U\left(\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& det\left(U{V}^{\mathrm{T}}\right)\end{array}\right)V^{\mathrm{T}}.$$

(32.9)

The diagonal matrix is a projection from the orthogonal group $\mathrm{O}\left(3\right)$ to the special orthogonal group $\text{SO}\left(3\right)$.

Last, we present a method [32.21] for n points that computes all local minima of the over constrained PnP problem. This involves solving the first derivatives explicitly with respect to the pose unknowns. To achieve this, following observation allows the elimination of the depths λ and the translation. Rigid transformation $\lambda \mathit{x}=\mathbf{R}\mathit{X}+\mathit{T}$ can be written for n points as a linear system for ${\lambda }_{j=\mathrm{1..}n}$ and the translation T

$$\left(\begin{array}{cccc}{\mathit{x}}_1& & & -I\\ & \mathrm{\ddots }& & \mathrm{⋮}\\ & & {\mathit{x}}_n& -I\end{array}\right)\left(\begin{array}{c}{\lambda }_1\\ \mathrm{⋮}\\ {\lambda }_n\\ \mathit{T}\end{array}\right)=\left(\begin{array}{c}\mathbf{R}{\mathit{X}}_1\\ \mathrm{⋮}\\ \mathbf{R}{\mathit{X}}_n\end{array}\right).$$

We can solve for the unknown depths-translation vector and back substitute it into a least squares minimization problem with respect to rotation parameters. It turns out that if we use the three Rodriguez parameters as rotation parametrization the necessary conditions for an extremum (vanishing derivatives) turn out to be three cubic equations [32.21]. Last we would like to point out to the reader that a nonlinear function of the rotation matrix can also be solved as an optimization problem on the Lie-group SO(3) [32.22, 32.23, 32.24] for the case of line correspondences.

1.3 Triangulation

When we know both the intrinsics and extrinsics or their summarization in matrix P and we measure a point we cannot recover its depth from just one camera position. Assuming that we have the projection of the same point X in two cameras

$${\lambda }_1{\mathit{u}}_1={\mathbf{P}}_1\left(\begin{array}{c}\mathit{X}\\ 1\end{array}\right),{\lambda }_2{\mathit{u}}_2={\mathbf{P}}_2\left(\begin{array}{c}\mathit{X}\\ 1\end{array}\right),$$

(32.10)

with known projection matrices ${\mathbf{P}}_1$ and ${\mathbf{P}}_2$ we can recover the position X in space, a process well known as triangulation. Observe that we can achieve triangulation without decomposing the projection matrices into intrinsic and extrinsic parameters, we neeed though to remove the distortion in order to write them as above.

Having correspondences of the same point in two cameras with known projection matrices P_l and P_r we can solve the two projection equations for the world point X. It is worth noting that each point provides two independent equations so that triangulation becomes an overconstrained problem for two views. This is not a contradiction since two rays do not intersect in general in space unless they satisfy the epipolar constraint as presented in the next paragraph. The following matrix in the left hand side has in general rank 4 unless the epipolar constraint is satisfied in which case it has rank 3.

$$\left(\begin{array}{c}x{\mathbf{P}}_{\mathrm{l}}(3,:)-{\mathbf{P}}_{\mathrm{l}}(1,:)\\ y{\mathbf{P}}_{\mathrm{l}}(3,:)-{\mathbf{P}}_{\mathrm{l}}(2,:)\\ x{\mathbf{P}}_{\mathrm{r}}(3,:)-{\mathbf{P}}_{\mathrm{r}}(1,:)\\ y{\mathbf{P}}_{\mathrm{r}}(3,:)-{\mathbf{P}}_{\mathrm{r}}(2,:)\end{array}\right)\left(\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right)=\mathbf{0},$$

(32.11)

where $\mathbf{P}(i,:)$ means the i-th row of matrix P.

Obviously, the homogeneous system above can be transformed into an inhomogeneous linear system with unknowns $(X,Y,Z)$. Otherwise it can be solved by finding the vector closest to the null-space of the 4 × 4 matrix above using SVD. A thorough treatment of triangulation is the classic [32.25].

1.4 Moving Stereo

Imagine now that a rigid stereo system consisting of cameras c_l (left) and c_r (right)

$${\mathit{u}}_{\mathrm{l}i}\approx {\mathbf{P}}_{\mathrm{l}}{\mathit{X}}_i,$$

(32.12)

$${\mathit{u}}_{\mathrm{r}i}\approx {\mathbf{P}}_{\mathrm{r}}{\mathit{X}}_i,$$

(32.13)

is attached to a moving robot and observe this system at two time instances

$${\mathit{X}}_0={\mathbf{R}}_1{\mathit{X}}_1+{\mathit{T}}_1,$$

(32.14)

where ${\mathit{X}}_0$ are point coordinates with respect to the world coordinate system, usually assumed aligned with one of the camera instances, and ${\mathit{X}}_1$ are the coordinates of the same point with respect to the camera rig, after a motion $({\mathbf{R}}_1,{\mathit{T}}_1)$. To estimate the motion of the rig, we have to solve two correspondence problems, first, between left and right image, and second, between left (or right) at the first time instance and left (or right, respectively) at the second time instance. The left to right correspondence enable the solution of the triangulation problem at each time instance. Motion can be obtained then by solving equations (32.14) for $({\mathbf{R}}_1,{\mathit{T}}_1)$, a problem called absolute orientation. Alternatively one can avoid the second triangulation and solve the pose estimation problem between triangulated points in 3-D and points in the left image only. The most popular visual odometry system today is libviso [32.26] and is based on a moving stereo rig ( ).

1.4.1 Absolute Orientation

The treatment for moving stereo will be short and the reader is referred to a similar treatment in the chapter about range sensing. We assume that correspondences between two time instances have been established based on tracking in the images so that we can formulate equations of the form

$${\mathit{X}}_2=\mathbf{R}{\mathit{X}}_1+\mathit{T}.$$

The standard way [32.20, 32.27] to solve this problem is to eliminate the translation by subtracting the centroids yielding

$${\mathit{X}}_2-\overline{{\mathit{X}}_2}=\mathbf{R}({\mathit{X}}_1-\overline{{\mathit{X}}_1}).$$

We need at least three points in total to obtain at least two noncollinear mean-free $\mathit{X}-\overline{\mathit{X}}$ vectors. If we concatenate the mean free vectors for n points into an $n\times 3$ matrix $A_{1,2}$ we can formulate the following minimization of the Frobenius norm

$$\underset{R\in \text{SO}\left(3\right)}{min}\parallel A_2-\mathbf{R}{A}_1{\parallel }_F,$$

which is known as the Procrustes problem. It can be shown [32.20] that the solution is obtained through SVD as in (32.9) where U, V are obtained from the singular value decomposition

$$A_2{A}_1^{\mathrm{T}}=US{V}^{\mathrm{T}}.$$

Solutions are usually obtained with RANSAC by sampling triples of points and verification with the Procrustes method.

1.5 Structure from Motion

Relax now the assumption that projection matrices are known and remain with measuring and matching corresponding points ${\mathit{u}}_1$ and ${\mathit{u}}_2$. This is the well known structure from motion problem or more precisely structure and 3-D-motion from 2-D motion. In photogrammetry, it is well known as relative orientation problem. Even after eliminating the λ’s from equations (32.12) or by writing them in projective equivalence form

$${\mathit{u}}_1\approx {\mathbf{P}}_1\left(\begin{array}{c}\mathit{X}\\ 1\end{array}\right),{\mathit{u}}_2\approx {\mathbf{P}}_2\left(\begin{array}{c}\mathit{X}\\ 1\end{array}\right),$$

(32.15)

we realize that we if $(\mathit{X},{\mathbf{P}}_1,{\mathbf{P}}_2)$ is a solution than $(\mathbf{H}\mathit{X},{\mathbf{P}}_1{\mathbf{H}}^{-1},{\mathbf{P}}_2{\mathbf{H}}^{-1})$ is a solution, too, where H an invertible 4 × 4 real matrix or in other words a collineation in ${\mathbb{P}}^3$. Even if we align the world coordinate system with the coordinate system of the first camera, which practice is common

$${\mathit{u}}_1\approx \left(\mathit{I},\mathbf{0}\right)\mathit{X},{\mathit{u}}_2\approx {\mathbf{P}}_2\mathit{X},$$

(32.16)

we remain with the same ambiguity where H is of the form

$$\mathbf{H}\approx \left(\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ h_{41}& h_{42}& h_{43}& h_{44}\end{array}\right),$$

(32.17)

with $h_{44}\ne 0$. This ambiguity is possible when the projection matrices are arbitrary rank 3 real matrices without any constraint on their elements. If we assume that we have calibrated our cameras then the projection matrices depend only on displacements

$${\mathit{u}}_1\approx \left(\mathbf{I},\mathbf{0}\right)\mathit{X},{\mathit{u}}_2\approx \left(\mathbf{R},\mathit{T}\right)\mathit{X},$$

(32.18)

and the only remaining ambiguity is the scale ambiguity where H looks like an identity matrix except $h_{44}=s\ne 1$ being the scale factor. In other words if $(\mathbf{R},\mathit{T},\mathit{X})$ is a solution then $(\mathbf{R},s\mathit{T},1/s\mathit{X})$ is a solution, too. These remarks generalize in multiple views. Because, in robotics the $(\mathbf{R},\mathit{T})$ matrices correspond to location and X to mapping of the environment, the problem has the more proper term GlossaryTerm

SLAM

: Simultaneous localization and mapping. However, because the term SLAM has been used with a variety of sensors like sonar and laser range scanners, the term monocular SLAM is better suited to describe structure from motion from multiple views [32.28].

1.5.1 Epipolar Geometry

This is probably one of the most studied problems in computer vision. We constrain ourselves to the calibrated case which is most relevant to robotics applications. The necessary and sufficient condition for the intersection of the two rays $\mathbf{R}{\mathit{x}}_1$ and ${\mathit{x}}_2$ is that the two rays are coplanar with the baseline T

$${\mathit{x}}_2^{\mathrm{T}}(\mathit{T}\times \mathbf{R}{\mathit{x}}_1)=0,$$

(32.19)

which is the epipolar constraint (Fig. 32.2). To avoid the scale ambiguity we assume that T is a unit vector. We proceed by summarizing the unknowns into one matrix

$$\mathbf{E}=\widehat{\mathbf{T}}\mathbf{R}$$

(32.20)

where $\widehat{\mathit{T}}$ is the 3 × 3 skew-symmetric matrix to the vector T. The E matrix is called the essential matrix. The epipolar constraint reads then

$${\mathit{x}}_2^{\mathrm{T}}\mathbf{E}{\mathit{x}}_1=0,$$

(32.21)

which is the equation of a line in the ${\mathit{x}}_2$ plane with coefficients $E{\mathit{x}}_1$ or a coefficient of a line in the ${\mathit{x}}_1$ plane with coefficients $E^{\mathrm{T}}{\mathit{x}}_2$. These lines are called epipolar and form pencils whose centers are the epipoles ${\mathit{e}}_1$ and ${\mathit{e}}_2$, in the first and second image plane respectively. The epipoles are the intersections of the baseline with the two image planes, hence ${\mathit{e}}_2\approx \mathit{T}$ and ${\mathit{e}}_1\approx -{\mathbf{R}}^{\mathrm{T}}\mathit{T}$. Looking at the equations of the epipolar lines we can immediately infer that $E^{\mathrm{T}}{\mathit{e}}_1=0$ and $E{\mathit{e}}_2=0$.

Fig. 32.2

A point is perspectively projected to calibrated image vectors $\mathbf{R}{\mathit{x}}_1$ and ${\mathit{x}}_2$ which are coplanar with baseline T

The set of all essential matrices

$$\begin{array}{rl}& \mathcal{E}=\left\{\mathbf{E},\in ,{\mathbb{R}}^{3\times 3},,\mid ,,\mathbf{E},=,\widehat{\mathit{T}},\mathbf{R},,\right.\\ & \left..\text{ where },\mathit{T},\in ,{\mathbb{S}}^2,\text{ and },\mathbf{R},\in ,\text{SO(3)}\right\}\end{array}$$

has been characterized as a manifold of dimension 5 [32.29]. It has been proven [32.30] that

Proposition 32.1

A matrix $\mathbf{E}\in {\mathbb{R}}^{3\times 3}$ is essential if and only if it has two singular values equal to each other and third singular value equal zero.

We present here Nister’s method [32.31] for recovering an essential matrix from five point correspondences and which gained in popularity because of its suitability for RANSAC methods.

1.5.2 Minimal Case

We expand the epipolar constraint in terms of homogeneous coordinates ${\mathit{x}}_1=(x_1,y_1,z_1)$ and ${\mathit{x}}_2=(x_2,y_2,z_2)$ (when the points are not at infinity $z_i=1$) and obtain

$$\left(x_1{\mathit{x}}_2^{\mathrm{T}},y_1{\mathit{x}}_2^{\mathrm{T}},z_1{\mathit{x}}_3^{\mathrm{T}}\right)E_{\mathrm{s}}=0,$$

(32.22)

where E_s is the raw by raw stacked version of matrix E. When we use only five point correspondences the resulting linear homogeneous system will have as a solution any vector in the four dimensional kernel of the data matrix

$$E_{\mathrm{s}}={\lambda }_1{\mathit{u}}_1+{\lambda }_2{\mathit{u}}_2+{\lambda }_3{\mathit{u}}_3+{\lambda }_4{\mathit{u}}_4.$$

(32.23)

At this point we want the matrix E resulting from E_s to be an essential matrix satisfying Proposition 32.1. It has been proven [32.30] that

Proposition 32.2

A matrix $\mathbf{E}\in {\mathbb{R}}^{3\times 3}$ is essential if and only if

$${\mathbf{EE}}^{\mathrm{T}}\mathbf{E}=\frac{1}{2}\text{trace}\left({\mathbf{EE}}^{\mathrm{T}}\right)\mathbf{E}.$$

(32.24)

Though the $det\left(\mathbf{E}\right)=0$ constraint can be inferred from (32.24) we are still going to use it together with (32.24) to obtain ten cubic equations in the elements of E. As described in [32.31], one can obtain a tenth degree polynomial in λ₄. The number of real roots of this polynomial are computed with a Sturm sequence. There is no proof beyond physical plausibility of the existence of at least one solution that a real root will exist at all. Several alternative 5-point solvers have been proposed since Nister’s paper [32.32, 32.33, 32.34, 32.35] and an extensive list including code has been established by Pajdla’s group [32.36].

Assuming that we have recovered an essential matrix from point correspondences, the next task is to recover an orthogonal matrix R and a unit vector translation T from the essential matrix. If $E=U\text{diag}(\sigma ,\sigma ,0)V^{\mathrm{T}}$, there are four solutions for the pair $(\widehat{T},R)$

$$\begin{array}{rl}(\widehat{{\mathit{T}}_1},R_1)& =(U{R}_{z,+\pi /2}\mathrm{\Sigma }{U}^{\mathrm{T}},U{R}_{z,+\pi /2}^{\mathrm{T}}{V}^{\mathrm{T}}),\\ (\widehat{{\mathit{T}}_2},R_2)& =(U{R}_{z,-\pi /2}\mathrm{\Sigma }{U}^{\mathrm{T}},U{R}_{z,-\pi /2}^{\mathrm{T}}{V}^{\mathrm{T}}),\\ (\widehat{{\mathit{T}}_1},R_2)& =(U{R}_{z,+\pi /2}\mathrm{\Sigma }{U}^{\mathrm{T}},U{R}_{z,-\pi /2}^{\mathrm{T}}{V}^{\mathrm{T}}),\\ (\widehat{{\mathit{T}}_2},R_1)& =(U{R}_{z,-\pi /2}\mathrm{\Sigma }{U}^{\mathrm{T}},U{R}_{z,+\pi /2}^{\mathrm{T}}{V}^{\mathrm{T}}),\end{array}$$

where R_z denotes rotation around the z-axis. The four solutions can be split into two two-fold ambiguities:

• Mirror ambiguity: If T is a solution, then −T is a solution, too. There is no way to disambiguate from the epipolar constraint: ${\mathit{x}}_2^{\mathrm{T}}\left(\right(-\mathit{T})\times R{\mathit{x}}_1)=0$.

• Twisted pair ambiguity: If R is a solution, then also $R_{T,\pi }R$ is a solution. The first image is twisted around the baseline 180 degrees.

These ambiguities are resolved by checking if depths of triangulated points are positive.

1.5.3 Critical Ambiguities

The approach with five point correspondences has a finite number of feasible (feasible means that they may produce multiple interpretations of structures in front of the camera) solutions when the points in the scene lie on a plane (a two fold ambiguity) [32.37] or when the points on the scene and the camera centers lie on a double sheet hyperboloid with the additional constraint that the camera centers lie symmetrically to the main generator of the hyperboloid [32.38]. These are inherent ambiguities which hold for any number of point correspondences when one seeks a solution for an exact essential matrix.

When someone is solving the linear least squares system for the essential matrix, a planar scene as well as the case of all points and the camera centers lying on a quadric causes a rank deficiency of the system and thus infinite solutions for E.

Beyond the ambiguous situations, there is a considerable amount of literature regarding instabilities in the two view problem. In particularly, it has been shown [32.37, 32.39, 32.40] that a small field of view and insufficient depth variation can cause an indeterminacy in the estimation of the angle between translation and optical axis. An additional small rotation can cause a confounding between translation and rotation [32.41]. Moreover, it has been shown, that there exist local minima close to the global minimum that can fool any iterative scheme [32.42, 32.43].

1.5.4 3-Point SfM

Minimal solutions based on 5 points are still too slow to be used on mobile platforms where additional information like a reference gravity vector might be obtained from an IMU. We present here a recent solution using a reference direction and only 3 points [32.44].

We are given three image correspondences from calibrated cameras, and a single directional correspondence like the gravity vector or a vanishing point. This problem is equivalent to finding the translation vector t and a rotation angle θ around an arbitrary rotation axis.

Let us choose the arbitrary rotation axis to be ${\mathbf{e}}_2=[0,1,0{]}^{\mathrm{T}}$. After taking the directional constraint into account, from the initial five parameters in the essential matrix, we now only have to estimate three. We can use the axis-angle parameterization of a rotation matrix to rewrite the essential matrix constraint as follows

$${{\mathbf{p}}_{2i}}^{\mathrm{T}}\tilde{E}{\mathbf{p}}_1=0,$$

(32.25)

where

$$\tilde{E}=\widehat{\tilde{\mathit{t}}}(I+\sin \theta {\widehat{\mathbf{e}}}_2+(1-\cos \theta \left){\widehat{\mathbf{e}}}_2^2\right),$$

and $\tilde{\mathit{t}}=(x,y,1)$.

Each image point correspondence gives us one such equation, for a total of three equations in three unknowns (elements of t and θ). To create a polynomial system, we set $s=\sin \theta$ and $c=\cos \theta$, and add the trigonometric constraint $s^2+c^2-1=0$, for a total of four equations in four unknowns. In order to reduce the number of unknowns, we choose the direction of the epipole by assuming that the translation vector $\tilde{\mathit{t}}$ has the form $[x,y,1{]}^{\top }$. This means that for each $\tilde{\mathit{t}}$ that we recover, $-\tilde{\mathit{t}}$ will also need to be considered as a possible solution.

Once we substitute for $\tilde{E}$ in (32.25), the resulting system of polynomial equations has the following form

$$a_{i1}xs+a_{i2}xc+a_{i3}ys+a_{i4}yc+a_{i5}x-a_{i2}s+a_{i1}c+a_{i6}=0$$

(32.26)

for i = 1,..,3, and the equation

$$s^2+c^2-1=\mathrm{0\hspace{0.33em}.}$$

(32.27)

This polynomial system can be solved in closed form and has up to four solutions. The total number of possible pose matrices arising from our formulation is therefore at most 8, when we take into account the fact that we have to consider the sign ambiguity in translation.

1.6 Multiple Views SfM

When we talk about simultaneous localization and mapping we obviously mean over a longer period of time. The question is how do we integrate additional frames in our 3-D motion estimation (localization) process.

To exploit multiple frames we introduce rank constraints [32.45]. We assume that the world coordinate system coincides with the coordinate system of the first frame and that a scene point is projected to x_i in the i-th frame and that its depth with respect to the 1st frame is λ₁

$${\lambda }_i{\mathit{x}}_i={\mathbf{R}}_i\left({\lambda }_1{\mathit{x}}_1\right)+{\mathit{T}}_i.$$

(32.28)

Taking the cross product with x_i and writing it for n frames yields a homogeneous system

$$\left(\begin{array}{cc}\widehat{{\mathit{x}}_2}{\mathbf{R}}_2{\mathit{x}}_1& \widehat{{\mathit{x}}_2}{\mathit{T}}_2\\ \mathrm{⋮}& \mathrm{⋮}\\ \widehat{{\mathit{x}}_n}{\mathbf{R}}_n{\mathit{x}}_1& \widehat{{\mathit{x}}_2}{\mathit{T}}_n\end{array}\right)\left(\begin{array}{c}{\lambda }_1\\ 1\end{array}\right)=0,$$

(32.29)

that has the depth of a point in the first frame as an unknown. The $3n\times 2$ multiple view matrix has to have rank one [32.46], a constraint that infers both the epipolar and the trifocal equations. The least squares solution for the depth can easily be derived as

$${\lambda }_1=-\frac{{\sum }_{i=1}^n({\mathit{x}}_i\times {\mathit{T}}_i{)}^{\mathrm{T}}({\mathit{x}}_i\times {\mathbf{R}}_i{\mathit{x}}_1)}{\parallel {\mathit{x}}_i\times {\mathbf{R}}_i{\mathit{x}}_1{\parallel }^2}.$$

(32.30)

Given a depth for each point we can solve for motion by rearranging the multiple views constraint (32.29) as

$$\left(\begin{array}{cc}{\lambda }_1^1{\mathit{x}}_1^{1T}\otimes \widehat{{\mathit{x}}_i^1}& \widehat{{\mathit{x}}_i^1}\\ \mathrm{⋮}& \mathrm{⋮}\\ {\lambda }_1^n{\mathit{x}}_1^{nT}\otimes \widehat{{\mathit{x}}_i^n}& \widehat{{\mathit{x}}_i^n}\end{array}\right)\left(\begin{array}{c}{\mathit{R}}_i^{\text{stacked}}\\ {\mathit{T}}_i\end{array}\right)=0,$$

(32.31)

where ${\mathit{x}}_i^n$ is the n-th image point in the i-th frame and ${\mathbf{R}}_i,{\mathit{T}}_i$ is the motion from 1st to the i-th frame and ${\mathit{R}}_i^{\text{stacked}}$ is the 12 × 1 vector of stacked elements of the rotation matrix R_i. Suppose that k is the 12 × 1 kernel (or closest kernel in a least squares sense) of the $3n\times 12$ matrix in the left hand side obtained through singular value decomposition and let us call A the 3 × 3 matrix obtained from the first 9 elements of k and a the vector of elements 10–12. To obtain a rotation matrix we follow the SVD steps in the solution of absolute orientation (32.14) to find the closest orthogonal matrix to an arbitrary invertible matrix.

1.6.1 Bundle Adjustment

On top of such an approach, a bundle adjustment [32.47] minimizes the sum of all deviations between image coordinates and the backprojections of the points to be reconstructed.

$${\text{arg min}}_{R^f,T^f,X_p}{ϵ}^{\mathrm{T}}{C}^{-1}ϵ,$$

minimized with respect to all $6(F-1)$ motions and $3N-1$ structure unknowns, where ϵ is the vector containing all errors

$$\begin{array}{rl}{ϵ}_p^f& =\left(x_p^f,-,\frac{R_{11}^f{X}_p+R_{12}^f{Y}_p+R_{13}^f{Z}_p+T_x}{R_{31}^f{X}_p+R_{32}^f{Y}_p+R_{33}^f{Z}_p+T_z}\right.\\ & \left..y_p^f,-,\frac{R_{21}^f{X}_p+R_{22}^f{Y}_p+R_{23}^f{Z}_p+T_y}{R_{31}^f{X}_p+R_{32}^f{Y}_p+R_{33}^f{Z}_p+T_z}\right)\end{array}$$

and C is the error covariance matrix. We will continue with the assumption that C = I.

Call the objective function $\mathrm{\Phi }\left(u\right)=ϵ\left(u{)}^{\mathrm{T}}ϵ\right(u)$ with u the vector of unknowns. Given a starting value for the vector of unknowns u we iterate with steps $\mathrm{\Delta }\mathrm{u}$ by locally fitting a quadratic function to $\mathrm{\Phi }\left(u\right)$

$$\mathrm{\Phi }(u+\mathrm{\Delta }\mathrm{u})=\mathrm{\Phi }\left(\mathrm{u}\right)+\mathrm{\Delta }{\mathrm{u}}^{\mathrm{T}}\mathrm{\nabla }\mathrm{\Phi }\left(\mathrm{u}\right)+\frac{1}{2}\mathrm{\Delta }{\mathrm{u}}^{\mathrm{T}}\mathbf{H}\left(\mathrm{u}\right)\mathrm{\Delta }\mathrm{u},$$

where $\mathrm{\nabla }\mathrm{\Phi }$ is the gradient and H is the Hessian of Φ. The minimum of this local quadratic is at $\mathrm{\Delta }\mathrm{u}$ satisfying

$$\mathbf{H}\delta u=-\mathrm{\nabla }\mathrm{\Phi }\left(u\right).$$

If $\mathrm{\Phi }\left(u\right)=ϵ\left(u{)}^{\mathrm{T}}ϵ\right(u)$ then

$$\mathrm{\nabla }\mathrm{\Phi }=2\sum _i{ϵ}_i\left(u\right)\mathrm{\nabla }{ϵ}_i(u{)}^{\mathrm{T}}=\mathbf{J}(u{)}^{\mathrm{T}}ϵ,$$

where the Jacobian J consists of elements

$${\mathbf{J}}_{ij}=\frac{\partial {ϵ}_i}{\partial u_j},$$

and the Hessian reads

$$\begin{array}{rl}\mathbf{H}& =2\sum _i\left(\mathrm{\nabla },{ϵ}_i,\left(,u,\right),\mathrm{\nabla },{ϵ}_i,(,u,{)}^{\mathrm{T}},+,{ϵ}_i,(,u,),\frac{{\partial }^2{ϵ}_i}{\partial u^2}\right)\\ & =2\left(\mathbf{J},\left(,u,{)}^{\mathrm{T}},\mathbf{J},\right(,u,),+,\sum _i,{ϵ}_i,(,u,),\frac{{\partial }^2{ϵ}_i}{\partial u^2}\right)\approx 2\mathbf{J}\left(u{)}^{\mathrm{T}}\mathbf{J}\right(u)\end{array}$$

by omitting quadratic terms inside the Hessian. This yields the Gauss–Newton iteration

$$\left({\mathbf{J}}^{\mathrm{T}}\mathbf{J}\right)\mathrm{\Delta }\mathrm{u}={\mathbf{J}}^{\mathrm{T}}ϵ,$$

involving the inversion of a $(6F+3N-7)\times (6F+3N-7)$ matrix. Bundle adjustment is about the art of inverting efficiently $\left({\mathbf{J}}^{\mathrm{T}}\mathbf{J}\right)$.

Let us split the unknown vector u into $u=(a,b)$ following [32.48] obtaining

• $6F-6$ motion unknowns a,

• $3P-1$ structure unknonws b,

and we will explain this case better if we assume two motion unknowns a₁ and a₂ corresponding to 2 frames, and 3 unknown points $b_1,b_2,b_3$.

For keeping symmetry in writing we do not deal here with the global reference and the global scale ambiguity.

The Jacobian for 2 frames and 3 points has 6 pairs of rows (one pair for each image projection) and 15 columns/unknowns

$$\mathbf{J}=\frac{\partial ϵ}{\partial (a,b)}=\left({\underbrace{\begin{array}{cc}{\mathbf{A}}_1^1& 0\\ 0& {\mathbf{A}}_1^2\\ {\mathbf{A}}_2^1& 0\\ 0& {\mathbf{A}}_2^2\\ {\mathbf{A}}_3^1& 0\\ 0& {\mathbf{A}}_3^2\end{array}}}_{\text{motion}},{\underbrace{\begin{array}{ccc}{\mathbf{B}}_1^1& 0& 0\\ {\mathbf{B}}_1^2& 0& 0\\ 0& {\mathbf{B}}_2^1& 0\\ 0& {\mathbf{B}}_2^2& 0\\ 0& 0& {\mathbf{B}}_3^1\\ 0& 0& {\mathbf{B}}_3^2\end{array}}}_{\text{structure}}\right),$$

with A matrices being 2 × 6 and B matrices being 2 × 3 being Jacobians of the error ${ϵ}_i^f$ of the projection of the i-th point in the f-th frame. We observe now a pattern emerging in ${\mathbf{J}}^{\mathrm{T}}\mathbf{J}$

$${\mathbf{J}}^{\mathrm{T}}\mathbf{J}=\left(\begin{array}{ccccc}{\mathbf{U}}^1& 0& {\mathbf{W}}_1^1& {\mathbf{W}}_2^1& {\mathbf{W}}_3^1\\ 0& {\mathbf{U}}^2& {\mathbf{W}}_1^2& {\mathbf{W}}_2^2& {\mathbf{W}}_3^3\\ ..& ..& {\mathbf{V}}_1& 0& 0\\ ..& ..& 0& {\mathbf{V}}_2& 0\\ ..& ..& 0& 0& {\mathbf{V}}_3\end{array}\right),$$

with the block diagonals for motion and structure separated. Let us rewrite the basic iteration $\left({\mathbf{J}}^{\mathrm{T}}\mathbf{J}\right)\mathrm{\Delta }\mathrm{u}={\mathbf{J}}^{\mathrm{T}}ϵ$ as

$$\left(\begin{array}{cc}\mathbf{U}& \mathbf{W}\\ {\mathbf{W}}^{\mathrm{T}}& \mathbf{V}\end{array}\right)\left(\begin{array}{c}\mathrm{\Delta }\mathrm{a}\\ \mathrm{\Delta }\mathrm{b}\end{array}\right)=\left(\begin{array}{c}{ϵ}_a^{\prime }\\ {ϵ}_b^{\prime }\end{array}\right),$$

and premultiply with

$$\left(\begin{array}{cc}I& {\mathbf{WV}}^{-1}\\ 0& I\end{array}\right)\left(\begin{array}{cc}\mathbf{U}& \mathbf{W}\\ {\mathbf{W}}^{\mathrm{T}}& \mathbf{V}\end{array}\right)\left(\begin{array}{c}\mathrm{\Delta }\mathrm{a}\\ \mathrm{\Delta }\mathrm{b}\end{array}\right)=\left(\begin{array}{cc}I& {\mathbf{WV}}^{-1}\\ 0& I\end{array}\right)\left(\begin{array}{c}{ϵ}_a^{\prime }\\ {ϵ}_b^{\prime }\end{array}\right)$$

We find out that motion parameters can be updated separately by inverting a $6F\times 6F$ matrix

$$(\mathbf{U}-{\mathbf{WV}}^{-1}{\mathbf{W}}^{\mathrm{T}})\mathrm{\Delta }\mathrm{a}={ϵ}_{\mathrm{a}}^{\prime }-{\mathbf{WV}}^{-1}{ϵ}_{\mathrm{b}}^{\prime }.$$

Each 3-D point can be updated separately by inverting a 3 × 3 matrix V

$$\mathbf{V}\mathrm{\Delta }\mathrm{b}={ϵ}_{\mathrm{b}}^{\prime }-{\mathbf{W}}^{\mathrm{T}}\mathrm{\Delta }\mathrm{a}$$

It is worth mentioning that bundle adjustment though extremely slow captures the correlation between motion estimates and structure (3-D points) estimates which is artificially hidden in the iterative scheme in (32.29).

The largest scale motion estimation and registration of views has been performed by Teller et al. [32.49] with a decoupled computation first of relative rotations and finally of relative translations. The above multiple view SfM techniques can also be applied in a sliding window mode in time. Davison et al. [32.28] showed the first real-time recursive approach by decoupling the direction of the viewing rays from the depth unknowns. For other recursive approaches the reader is referred to the corresponding SLAM chapter.

2 3-D Vision for Grasping

In this section we will move from the basic geometry required for grasping to the main 3-D vision challenges associated with the limited knowledge we might have about the shape of the object as well as the actual selection of 3-D grasping poses.

Naturally, object grasping and manipulation is closely related to general scene understanding and problems such as object detection, recognition, categorization and pose estimation. Taking all the above, there are very few approaches that address all the problems in a single system. One example, reported in [32.50], addresses the problem of enabling transfer of grasp knowledge between object categories, defined using both their physical properties and functionality. This is a challenging problem given that a number of objects with similar physical properties afford different tasks. An example can be a screwdriver and a carrot that are structurally alike, but only the former can be used as a tool, or a ball and an orange where only the latter affords eating (Fig. 32.3).

Fig. 32.3

Examples of physically similar objects that afford different tasks

In relation to object grasping in particular, there are methods that assume that full 3-D model of the object is available and concentrate on grasp synthesis solely. In addition, many of the approaches conduct experiments in a simulated environment without working with real sensory data. However, the knowledge generated in simulation can also be applied later onto sensory data. Another group of approaches

considers grasp synthesis on real sensory data directly, dealing with problems such as noise, occlusions and missing data.

If the object to be grasped is known, there are approaches that store a database of grasp hypotheses, generated either in simulation or through experiments in a real setting. Most of the approaches assume that a 3-D mesh of the object is available and the challenge is then to automatically generate a set of feasible grasp hypotheses. This involves sampling the infinite space of possible hand configurations and ranking the resulting grasps according to some quality metric.

To simplify the process, a common approach is to approximate object’s shape with a constellation of primitives such as spheres, cones, cylinders, boxes or superquadrics [32.51, 32.52, 32.53, 32.54, 32.55]. The purpose of using shape primitives is to reduce the number of candidate grasps and thus prune the search space for finding the optimal set of grasp hypotheses.

One example, shown in Fig. 32.4 and reported in [32.52], decomposes a point cloud from a stereo camera into a constellation of boxes. Grasp planning is performed directly on the boxes which reduces the number of potential grasps. El-Khoury and Sahbani [32.56] distinguish between graspable and nongraspable parts of an object where each part is represented by fitting a superquadric to the point cloud data. Pelossof et al. [32.57] approximate an object with a single superquadric and use a Support Vector Machines based approach to search for the grasp that maximizes the grasp quality. Boularias et al. [32.58] model an object as a Markov random field (GlossaryTerm

MRF

) in which the nodes are points from the point cloud and edges are spanned between the six nearest neighbors of a point. A node in the MRF carries either one of the two labels: a good or a bad grasp location. Detry et al.[32.59] model the object as a constellation of local multimodal contour descriptors. The set of associated grasp hypotheses is modeled as a nonparametric density function in the space of six-dimensional (GlossaryTerm

6-D

) gripper poses, referred to as a bootstrap density. Papazov et al. [32.60] demonstrates 3-D object recognition and pose estimation in a grasping scenario considering cluttered scenes. Weisz and Allen [32.61] proposes a metric suitable for predicting grasp stability under pose uncertainty.

Fig. 32.4

Generation of grasp candidates through object shape approximation and decomposition from (after [32.52])

There are several approaches that deal specifically with incomplete point clouds. Marton et al. [32.63] exploit symmetry by fitting a curve to a cross section of the point cloud. Rao et al. [32.64] concentrates of depth segmentation and sample grasp points from the surface of a segmented object using surface normals. Bohg et al. [32.65] presents a related approach that reconstructs full object shape assuming planar symmetry and generates grasps based on the global shape of the object. Bone et al. [32.66] makes no prior assumption about the shape of the object and apply shape carving for generating a parallel-jaw gripper grasps. Hsiao et al. [32.67] employs heuristics for generating grasp hypotheses dependent on the shape of the point cloud. Recent work in [32.68] identifies regions that afford force closure grasps by evaluating local curvature of the objects to create an initial opposing grasp with two or three fingers, dependent on the relative size of the object with respect to the hand. Richtsfeld and Vincze [32.69] uses a stereo-camera setup to generate a 3-D representation of a scene with several objects and then generates various top grasps on object candidates. Maldonado et al. [32.70] use time-of-flight range data, model objects using 3-D Gaussians and rely on finger torque information during grasping to monitor the grasp execution. Stückler et al. [32.71] generate grasp hypotheses based on eigenvectors of the object’s footprints that are generated by projecting the 3-D object point cloud onto the supporting surface. The work of [32.72] presents a system for general scene understanding used for grasp planning and execution. The system uses a bottom-up grouping approach where contour and surface structures are used as the basis for grasp planning. The work builds upon previous work presented in [32.73].

Most of the recent work concentrates on grasp generalization either by observing human grasping or through off- and on-line learning directly on the robot. Kroemer et al. [32.74] demonstrates generalization capabilities using a pouring task scenario. The goal of the approach is to find a part of the object that is most likely to afford the demonstrated action. The learning method is based on the kernel logistic regression. Herzog et al. [32.75] stores a set of local templates of object that a human is interacting with. If a local part of an object segmented online is similar to a template in the database, the associated grasp hypothesis is executed. Song et al. [32.62] approach the problem of inferring a full grasp configuration in relation to a specific task the object is intended for. As in [32.76], the joint distribution over various grasping variables is modeled as a Bayesian network. Additional variables like task, object category and task constraints are introduced. The structure of this model is learned given a large number of grasp examples generated in a simulator and annotated with grasp quality metrics as well as suitability for a specific task. The learned quality of grasps on specific objects given a task is visualized in Fig. 32.5.

Fig. 32.5

Ranking of approach vectors on different objects given a specific task. The brighter an area the higher the rank. The darker an area, the lower the rank (after [32.62])

3 Conclusion and Further Reading

As main additional sources of reading, we recommend the textbooks by Hartley and Zisserman [32.12], Ma et al. [32.46], Faugeras [32.77], and Faugeras and Luong [32.11]. The reader is referred to Chap. 5 for fundamentals of estimation, to Chap. 35 for sensor fusion, to Chap. 34 for visual servoing, to Chap. 31 for range sensing, to Chap. 45 for 3-D models of the world, and to Chap. 46 for SLAM.

3-D vision is a rapidly advancing field and in this chapter we have covered only geometric approaches based on RGB cameras. Although depth sensors will become ubiquitous indoors and might be outdoors as well, RGB cameras remain formidable because of the higher number and larger diversity of features that can be matched and used for pose estimation and 3-D-modelling. Long range sensing can still be covered from motion with large translation while active sensors are constrained in terms of energy reflected from the environment.

Video-References

:

Google’s project Tango available from http://handbookofrobotics.org/view-chapter/32/videodetails/120

:

Finding paths through the world’s photos available from http://handbookofrobotics.org/view-chapter/32/videodetails/121

:

LIBVISO: Visual odometry for intelligent vehicles available from http://handbookofrobotics.org/view-chapter/32/videodetails/122

:

Parallel tracking and mapping for small AR workspaces (PTAM) available from http://handbookofrobotics.org/view-chapter/32/videodetails/123

:

DTAM: Dense tracking and mapping in real-time available from http://handbookofrobotics.org/view-chapter/32/videodetails/124

:

3-D models from 2-D video – automatically available from http://handbookofrobotics.org/view-chapter/32/videodetails/125