715

Supplementary Information
A Installing the Toolboxes – 716

B Linear Algebra – 717

C Geometry – 725

D Lie Groups and Algebras – 740

E Linearization, Jacobians and Hessians – 745

F Solving Systems of Equations – 750

G Gaussian Random Variables – 759

H Kalman Filter – 762

I Graphs – 768

J Peak Finding – 770

References – 774

Index of People – 789

Index of Functions, Classes and Methods – 791

Index of Models – 799

General Index – 801

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2023

P. Corke, Robotics, Vision and Control, Springer Tracts in Advanced Robotics 146,
https://doi.org/10.1007/978-3-031-06469-2
716 A Installing the Toolboxes

A Installing the Toolboxes

The most up-to-date instructions on installing the required software, and getting
started, can always be found at

> https://github.com/petercorke/RVC3-python

A.1 Installing the Packages

This book depends on the following open-source Python packages:

4 Robotics Toolbox for Python roboticstoolbox
4 Machine Vision Toolbox for Python machinevisiontoolbox

which in turn have dependencies on other packages created by the author and third
parties.
The Python package rvc3python provides a simple one-step installation of
these Toolboxes and their dependencies
$ pip install rvc3python

or
$ conda install rvc3python

The package provides additional resources for readers of the book including:
4 rvctool, a command line script that is an IPython wrapper. It imports the above
mentioned packages using import * and then provides an interactive com-
puting environment. By default rvctool has prompts like the regular Python
REPL not IPython, and it automatically displays the results of expressions like
MATLAB® does – put a semicolon on the end of the line to suppress that.
rvctool allows cutting and pasting in lines from the book, and prompt charac-
ters are ignored.
4 Jupyter notebooks, one per chapter, containing all the code examples.
4 The code to produce every Python-generated figure in the book.
4 All example scripts.
4 All block diagram models.

The GitHub repository RVC3-python also contains online renderable versions of

selected 3D figures, and all line drawings in EPS format.

A.2 Block Diagram Models

Block diagram models are simulated using the Python package bdsim which can
run models:
4 written in Python using bdsim block classes and wiring methods.
4 created graphically using bdedit and saved as a .bd (JSON format) file.

A.3 Getting Help

The GitHub repository RVC3-python hosts a number of other resources for users:
4 Wiki pages provides answers to frequently asked questions.
4 Discussions between users on aspects of the book and the core toolboxes.
4 Issues can be used for reporting and discussing issues and bugs.
 717
B Linear Algebra

B Linear Algebra

B.1 Vectors

The term vector has multiple meanings which can lead to confusion:
4 In computer science, a vector is an array of numbers or a tuple.
4 In physics and engineering, a vector is a physical quantity like force or velocity
which has a magnitude, direction and unit.
4 In mathematics, a vector is an object that belong to a vector space.
4 In linear algebra, a vector is a one-dimensional matrix organized as either a row
or a column.

In this book, we use all these interpretations and rely on the context to disambiguate,
but this section is concerned with the last two meanings. We consider only real vec-
tors which are an ordered n-tuple of real numbers which is usually written as

v D .v0 ; v1 ; : : : ; vn1 / 2 Rn

where v0 , v1 , etc. are called the scalar components of v, and vi is called the i th com-
ponent of v. The symbol Rn D R  R   R is a Cartesian product that denotes
the set of ordered n-tuples of real numbers. For a 3-vector, we often write the ele-
ments as v D .vx ; vy ; vz /.
The coordinate of a point in an n-dimensional space is also represented by an
n-tuple of real numbers. A coordinate vector is that same tuple but interpreted as
a linear combination v D v0 e 0 C v1 e 1 C C vn1 e n1 of the orthogonal basis
vectors fe 0 ; e 1 ; : : : ; e n1 g of the space – this is a vector from the origin to the
point. In this book, the basis vectors are denoted by fx; O yg
O or fx;
O y;
O zO g for 2 or 3
dimensions respectively.
In mathematics, an n-dimensional vector space (also called a linear space) is
a group-like object that contains a collection of objects called vectors v 2 Rn . It
supports the operations of vector addition a C b D .a0 C b0 ; a1 C b1 ; : : : ; an1 C
bn1 /, and multiplication (“scaling”) by a number s called a scalar sa D .sa0 ; sa1 ;
: : : ; san1 /. The negative of a vector, scaling by 1, is obtained by negating each
element of the vector a D .a0 ; a1 ; : : : ; an1 /.
The symbol Rn is used to denote a space of points or vectors, and context is
needed to resolve the ambiguity. We need to be careful to distinguish points and
vectors because the operations of addition and scalar multiplication, while valid for
vectors, are meaningless for points. We can add a vector to the coordinate vector of
a point to obtain the coordinate vector of another point, and we can subtract one co-
ordinate vector from another, and the result is the displacement between the points.
For many operations in linear algebra, it is important to distinguish between
column and row vectors
0 1
v0
B v1 C
B C
v D B : C or v D .v0 ; v1 ; : : : ; vn1 /
@ :: A
vn1
which are equivalent to an n  1 and a 1  n matrix (see next section) respec-
tively. Most vectors in this book are column vectors which are sometimes written
compactly as .v0 ; v1 ; : : : ; vn1 /> , where > denote a matrix transpose. Both can
be denoted by Rn , or distinguished by Rn1 or R1n for column and row vectors
respectively.
718 B Linear Algebra

The magnitude or length of a vector is a nonnegative scalar given by its p-norm

!1=p
Xn1
np.linalg.norm(v) kvkp D jvi j p
2 R0 :
i D0

The Euclidean length of a vector is given by kvk2 which is also referred to as the
L2 norm and is generally assumed when p is omitted, for example kvk. A unit
The unit vector corresponding to the vector 9 is one where kvk2 D 1 and is denoted as v.O The L1 norm is the sum of the
vector v is vO D kvk
1
2
v. absolute value of the elements of the vector, and is also known as the Manhattan
distance, it is the distance traveled when confined to moving along the lines in a
grid. The L1 norm is the maximum element of the vector.
The dot or inner product of two vectors is a scalar

X
n1

np.dot(a, b) a bDb aD ai bi D kak kbk cos 

i D0

where  is the angle between the vectors. a b D 0 when the vectors are orthogonal.
If a and b are column vectors, a; b 2 Rn1 , the dot product can be written as
a b D a> b D b> a

The outer product ab> 2 Rnn has a maximum rank of one, and if b D a is a
symmetric matrix.
For 3-vectors, the cross product
0 1
xO yO zO
np.cross(a, b) a  b D b  a D det@a0 a1 a2 A D kak kbk sin  nO
b0 b1 b2
where xO is a unit vector parallel to the x-axis, etc., and nO is a unit vector normal to
the plane containing a and b whose direction is given by the right-hand rule. If the
vectors are parallel, a  b D 0.

B.2 Matrices

Real matrices are a subset of all In this book we are concerned, almost exclusively, with real m  n matrices 9
matrices. For the general case of 0 1
complex matrices, the term Hermitian a0;0 a0;1 a0;n1
B a1;0 a1;1 a1;n1 C
is the analog of symmetric, and unitary B C
ADB : : : :: C 2 Rmn
is the analog of orthogonal. AH denotes @ :: :: :: : A
the Hermitian transpose, the complex
am1;0 am1;1 am1;n1
conjugate transpose of the complex
matrix A. Matrices are rank-2 tensors. with m rows and n columns. If n D m, the matrix is square.
Matrices of the same size A 2 Rmn and B 2 Rmn can be added
A+B C D A C B; ci;j D ai;j C bi;j ; i D 0; :::; m1; j D 0; :::; n1
or multiplied element-wise
A*B C D A ı B; ci;j D ai;j bi;j ; i D 0; :::; m1; j D 0; :::; n1
which is also called the Hadamard product. Two matrices with conforming dimen-
sions A 2 Rmn and B 2 Rnp can be multiplied

X
n1
A@B C D AB; ci;j D ai;k bk;j ; i D 0; :::; m1; j D 0; :::; p1
kD0

which is matrix multiplication and C 2 Rmp .

 719
B.2 Matrices

all matrices

complex real

non square square

general SE(n) normal

det≠0 det=0

invertible singular symmetric orthogonal O(n) skew-symmetric

det=+1 det=-1

+ve de nite SO(n)

diagonal

identity

. Fig. B.1 Taxonomy of matrices. Matrices shown in blue are never singular

The transpose of a matrix is A.T

B D A> ; bi;j D aj;i ; 8i; j

and it can be shown that

.AB/> D B> A> ; .ABC/> D C> B> A> ; etc:

The elements of a matrix can also be matrices, creating a block or partitioned

matrix
0 1
B0;0 B0;1 B0;n1
B B1;0 B1;1 B1;n1 C
B C
ADB : : : :: C
@ : : :
: : : : A
Bm1;0 Bm1;1 Bm1;n1
where all matrices in a column must have the same width, and all matrices in a row
must have the same height. A block diagonal matrix has blocks, not necessarily of
the same size, arranged along the diagonal
0 1
B0
B 0 C
B B1 C
ADB :: C
@ : A
0
Bp1

A taxonomy of matrices is shown in . Fig. B.1.

720 B Linear Algebra

B.2.1 Square Matrices

np.linalg.inv(A) A square matrix may have an inverse A1 in which case

AA1 D A1 A D 1nn

where
0 1
1
B 0 C
B 1 C
np.eye(n) 1nn D B :: C 2 Rnn
@ : A
0
1

is the identity matrix, a unit diagonal matrix, sometimes written as I. The in-
verse A1 exists provided that the matrix A is non-singular, that is, its determinant
np.linalg.det(A) det.A/ ¤ 0. The inverse can be computed from the matrix of cofactors. If A and B
are square and non-singular, then

.AB/1 D B1 A1 ; .ABC/1 D C1 B1 A1 ; etc:

and also
 > 1  1 >
A D A D A> :

The inverse can be written as

1
A1 D adj.A/
det.A/

where adj.A/ is the transpose of the matrix of cofactors and known as the adjugate
Confusingly, sometimes also referred matrix and sometimes denoted by A . 9 If A is non-singular, the adjugate can be
to as the adjoint matrix, but in this computed by
book that term is reserved for the
matrix introduced in 7 Sect. 3.1.3. adj.A/ D det.A/A1 :

For a square n  n matrix, if:

A D A> – the matrix is symmetric. The inverse of a symmetric matrix is also
symmetric. Many matrices that we encounter in robotics are symmetric, for exam-
ple, covariance matrices and manipulator inertia matrices.
A1 D A> – the matrix is orthogonal. The matrix is also known as orthonormal
since its column (and row) vectors must be of unit length, and the columns (and
rows) are orthogonal (normal) to each other, that is, their dot products are zero.
The product of two orthogonal matrices is also an orthogonal matrix. The set of
n  n orthogonal matrices forms a group O.n/ under the operation of matrix multi-
plication known as the orthogonal group. The determinant of an orthogonal matrix
is either C1 or 1. The subgroup of orthogonal matrices with determinant +1 is
called the special orthogonal group denoted by SO.n/.
A D A> – the matrix is skew symmetric or anti symmetric. Such a matrix has
a zero diagonal, and is always singular if n is odd. Any matrix can be written as
the sum of a symmetric matrix and a skew-symmetric matrix.
There is a mapping from a vector to a skew-symmetric matrix which, for the case
v 2 R3 , is
0 1
0 vz vy
skew(v) A D Œv D @ vz 0 vx A (B.1)
vy vx 0
 721
B.2 Matrices

The inverse mapping is

v D _ .A/ : vex(A)

If v 2 Rn and A 2 Rnn then ŒAv D det.A/A > Œv A1 .

For the 3-dimensional case, the cross product can be written as the matrix-vector
product v1  v2 D Œv1  v2 , and v> Œv D Œv v D 0, 8v. If R 2 SO.3/ then ŒRv D
RŒv R> .
A> A D AA> – the matrix is normal and can be diagonalized by an orthogonal
matrix U so that U> AU is a diagonal matrix. All symmetric, skew-symmetric and
orthogonal matrices are normal matrices as are matrices of the form A D B> B D
BB> where B is an arbitrary matrix.

The square matrix A 2 Rnn can be applied as a linear transformation to a vector

x 2 Rn

x 0 D Ax

which results in another vector, generally with a change in its length and direction.
For example, in two dimensions, if x is the set of all points lying on a circle, then
x 0 defines points that lie on some ellipse. However, there are some important spe-
cial cases. If A 2 SO.n/, the transformation is isometric and the vector’s length is
unchanged, kx 0 k D kxk. The (right) eigenvectors of the matrix are those vectors x np.linalg.eig(A)
such that

Ax D i x; i D 0; : : : ; n  1 (B.2)

that is, their direction is unchanged when transformed by the matrix. They are
simply scaled by i , the corresponding eigenvalue. The matrix A has n eigenvalues
(the spectrum of the matrix) which can be real or complex pairs. For an orthogonal
matrix, the eigenvalues lie on a unit circle in the complex plane, j i j D 1, and the
eigenvectors are all orthogonal to one another.
The Toolbox demo
>>> %run -m eigdemo 1 2 3 4

shows an animation of a rotating 2D coordinate vector x and the transformed vector

Ax. The arguments are the row-wise elements of A. Twice per revolution x and
Ax are parallel – these are the eigenvectors of A.
If A is non-singular, then the eigenvalues of A1 are the reciprocal of those
of A, and the eigenvectors of A1 are parallel to those of A. The eigenvalues of A>
are the same as those of A but the eigenvectors are different.
The trace of a matrix is the sum of the diagonal elements

X
n1
tr.A/ D ai;i np.trace(A)
i D0

which is also the sum of the eigenvalues

X
n1
tr.A/ D i :
i D0

For a rotation matrix, either 2D or 3D, the trace is related to the angle of rotation

tr.R/  1
 D cos1
2
722 B Linear Algebra

about the rotation axis, but due to the limited range of cos1 , values of  above
cannot be properly determined.
The determinant of the matrix is equal to the product of the eigenvalues

Y
n1
det.A/ D i
i D0

thus a matrix with one or more zero eigenvalues will be singular.

The eigenvalues of a real symmetric matrix A are all real and we classify such
a matrix according to the sign of its eigenvalues:
4 i > 0; 8i, positive-definite, never singular, written as A  0
4 i  0; 8i, positive-semi-definite, possibly singular, written as A < 0
4 i < 0; 8i, negative-definite, never singular, written as A  0
4 otherwise, indefinite.

The diagonal elements of a positive-definite matrix are positive, ai;i > 0, and its
determinant is positive det.A/ > 0. The inverse of a positive-definite matrix is also
positive-definite.
We will frequently encounter the matrix quadratic form

s D x > Ax (B.3)

which is a scalar. If A is positive-definite, then s > 0, 8x ¤ 0. For the case that

A is diagonal this can be written

X
n1
sD ai;i xi2
i D0

which is a weighted sum of squares. If A D 1 then s D .kvk2 /2 . If A is symmetric

then
X
n1 X
n1 X
n1
sD ai;i xi2 C 2 ai;j xi xj
i D0 i D0 j Di C1

and the result also includes products or correlations between elements of x.

The Mahalanobis distance is a weighted distance or norm
p
s D x > P1 x

where P 2 Rnn is a covariance matrix which down-weights elements of x where

uncertainty is high.
The matrices A> A and AA> are always symmetric and positive-semi-definite.
This implies than any symmetric matrix A can be written as

A D LL>

np.linalg.cholesky(A) where L is the Cholesky decomposition of A.

sp.linalg.sqrtm(A) Other matrix factorizations of A include the matrix square root

A D SS
1
where S is the square root of A or A 2 which is positive definite (and symmetric) if
np.linalg.qr(A) A is positive definite, and QR-decomposition

A D QR

where Q is an orthogonal matrix and R is an upper triangular matrix.

 723
B.2 Matrices

If T is any non-singular matrix, then

A D TBT1

is known as a similarity transformation or conjugation. A and B are said to be sim-

ilar, and it can be shown that the eigenvalues are unchanged by this transformation.
The matrix form of (B.2) is

AX D Xƒ

where X 2 Rnn is a matrix of eigenvectors of A, arranged column-wise, and ƒ is

a diagonal matrix of corresponding eigenvalues. If X is non-singular, we can rear-
range this as

A D XƒX1

which is the eigen or spectral decomposition of the matrix. This implies that the
matrix can be diagonalized by a similarity transformation

ƒ D X1 AX :

If A is symmetric, then X is orthogonal and we can instead write

A D XƒX> : (B.4)

The determinant of a square matrix A 2 Rnn is the factor by which the transfor-
mation scales volumes in an n-dimensional space. For two dimensions, imagine
a shape defined by points x i with an enclosed area ƒ. The shape formed by
the points Ax i would have an enclosed area of ƒdet.A/. If A is singular, the
points Ax i would be coincident or collinear and have zero enclosed area. In a
similar way for three dimensions, the determinant is a scale factor applied to the
volume of a set of points transformed by A.
The columns of A D .c 0 ; c 1 ; : : :; c n1 / can be considered as a set of vectors that
define a space – the column space. Similarly, the rows of A can be considered as
a set of vectors that define a space – the row space. The column rank of a matrix np.linalg.matrix_rank(A)
is the number of linearly independent columns of A. Similarly, the row rank is the
number of linearly independent rows of A. The column rank and the row rank are
always equal and are simply called the rank of A, and this has an upper bound of
n. A square matrix for which rank.A/ < n is said to be rank deficient or not of full
rank. The rank shortfall n  rank.A/ is the nullity of A. In addition, rank.AB/
min.rank.A/; rank.B// and rank.A C B/ rank.A/ C rank.B/. If x is a column
vector, the matrix xx > , the outer product of x, has rank 1 for all x ¤ 0.

B.2.2 Non-Square and Singular Matrices

Singular square matrices can be treated as a special case of a non-square matrix.

For a non-square matrix A 2 Rmn the rank is the dimension of the largest non-
singular square submatrix that can be formed from A, and has an upper bound of
min.m; n/.
A non-square or singular matrix cannot be inverted, but we can determine the
left generalized inverse or pseudoinverse or Moore-Penrose pseudoinverse

AC A D 1nn

where AC D .A> A/1 A> . The right generalized inverse is

AAC D 1mm
724 B Linear Algebra

where AC D A> .AA> /1 . The qualifier left or right denotes which side of A the
pseudoinverse appears.
If the matrix A is not of full rank, then it has a finite null space or kernel. A
vector x lies in the null space of the matrix if

sp.linalg.null_space(A) Ax D 0 :

More precisely, this is the right-null space. A vector lies in the left-null space if

x>A D 0 :

The left-null space is equal to the right-null space of A> .

The null space is defined by a set of orthogonal basis vectors whose cardinality
is the nullity of A. Any linear combination of these null-space basis vectors lies in
the null space.
For a non-square matrix A 2 Rmn , the analog to (B.2) is

Avi D i ui

where ui 2 Rm and vi 2 Rn are respectively the right and left singular vectors
of A, and i its singular values. The singular values are nonnegative real numbers
that are the square root of the eigenvalues of AA> and ui are the corresponding
eigenvectors. vi are the eigenvectors of A> A.
The singular value decomposition or SVD of the matrix A is

np.linalg.svd(A) A D U†V>

where U 2 Rmm and V 2 Rnn are both orthogonal matrices comprising, as

columns, the corresponding singular vectors ui and vi . † 2 Rmn is a diagonal
matrix of the singular values i in decreasing magnitude
0 1
0
B :: C
B : 0 C
B C
B C
B r1 C
†DB C
B 0 C
B C
B 0 :: C
@ : A
0

where r D rank.A/ is the rank of A and i C1 i . For the case where r < n, the
diagonal will have n  r zero elements as shown. Columns of V corresponding
np.linalg.cond(A) to the zero columns of † define the null space of A. The condition number of a
matrix A is max. /= min. / and a high value means the matrix is close to singular
or “poorly conditioned”.
 725
C Geometry

C Geometry

Geometric concepts such as points, lines, ellipses and planes are critical to the
fields of robotics and robotic vision. We briefly summarize key representations in
both Euclidean and projective (homogeneous coordinate) space.

C.1 Euclidean Geometry

C.1.1 Points

A point in an n-dimensional space is represented by an n-tuple, an ordered set of

n numbers .x0 ; x1 ; : : : ; xn1 /> which define the coordinates of the point. The tuple
can also be interpreted as a column vector – a coordinate vector – from the origin to
the point. A point in 2-dimensions is written as p D .x; y/> , and in 3-dimensions
is written as p D .x; y; z/> .

C.1.2 Lines

C.1.2.1 Lines in 2D
A line is defined by ` D .a; b; c/> such that

ax C by C c D 0 (C.1)

which is a generalization of the line equation we learned in school y D mx C c, but

which can easily represent a vertical line by setting b D 0. v D .a; b/ is a vector
normal to the line, and v D .b; a/ is a vector parallel to the line. The line that
joins two points p 1 and p 2 , p i D .xi ; yi /, is given by the solution to Line2.Join

!0a1
x1 y1 1 @ A
b D0
x2 y2 1
c

which is found from the right null space of the leftmost term. The intersection point
of two lines ` 1 and ` 2 is l1.intersects(l2)

! ! !
a1 b1 x c
D 1
a2 b2 y c2

which has no solution if the lines are parallel – the leftmost term is singular.
We can also represent the line in polar form

x cos  C y sin  C D0

where  is the angle from the x-axis to the line and is the normal distance between
the line and the origin, as shown in . Fig. 12.20.

C.1.2.2 Lines in 3D and Plücker Coordinates

We can define a line by two points, p and q, as shown in . Fig. C.1, which would
require a total of six parameters ` D .px ; py ; pz ; qx ; qy ; qz /. However, since these
points can be arbitrarily chosen, there would be an infinite set of parameters that
represent the same line making it hard to determine the equivalence of two lines.
726 C Geometry

. Fig. C.1 Describing a line in three dimensions

There are advantages in representing a line as

` D .!  q; p  q/ D .v; !/ 2 R6
This is not intuitive but consider two
parallel planes and an arbitrary 3D line where ! is the direction of the line and v is the moment of the line – a vector
passing through them. The line can be from the origin to a point on the line and which is normal to the line. This is a
described by the 2-dimensional Plücker coordinate vector – a 6-dimensional quantity subject to two constraints:
coordinates of its intersection point on the coordinates are homogeneous and thus invariant to overall scale factor; and
each plane – a total of four coordinates. v ! D 0. Lines therefore have 4 degrees-of-freedom 9 and the Plücker coordinates
lie on a 4-dimensional manifold – the Klein quadric – in 6-dimensional space.
Ideal as in imaginery, not as in perfect. Lines with ! D 0 lie at infinity and are known as ideal lines. 9
We will first define two points
>>> P = [2, 3, 4]; Q = [3, 5, 7];

and then create a Plücker line object

>>> L = Line3.Join(P, Q)
{ 1 -2 1; -1 -2 -3}

which displays the v and ! components. These can be accessed as properties

>>> L.v.T
array([ 1, -2, 1])
>>> L.w.T
array([ -1, -2, -3])

A Plücker line can also be represented as a skew-symmetric matrix

>>> L.skew()
array([[ 0, 1, 2, -1],
[ -1, 0, 1, -2],
[ -2, -1, 0, -3],
[ 1, 2, 3, 0]])

Since lines have infinite length, we To plot this line, we first define a region of 3D space 9 then plot it in blue
need to specify a finite volume in
>>> plotvol3([-5, 5]);
which to draw it. >>> L.plot("b");

The line is the set of all points

v!
p. / D C !; 2R
! !
which can be generated parametrically in terms of the scalar parameter
>>> L.point([0, 1, 2])
array([[ 0.5714, 0.3042, 0.03691],
[ 0.1429, -0.3917, -0.9262],
[ -0.2857, -1.087, -1.889]])

where the columns are points on the line corresponding to D 0; 1; 2.

 727
C.1 Euclidean Geometry

A point x is closest to the line when

.x  q/ !
D :
! !

For the point .1; 2; 3/> , the closest point on the line, and its distance, is given by
>>> [x, d] = L.closest_to_point([1, 2, 3])
>>> x
array([ 1.571, 2.143, 2.714])
>>> d
0.6547

The line intersects the plane n> x C d D 0 at the point coordinate

v  n  d!
xD :
! n
For the xy-plane, the line intersects at
>>> p, _ = L.intersect_plane([0, 0, 1, 0])
>>> p
array([ 0.6667, 0.3333, 0])

Two lines can be identical, coplanar or skewed. Identical lines have linearly
dependent Plücker coordinates, that is, ` 1 D ` 2 or Ò 1 D Ò 2 . If coplanar, they can
be parallel or intersecting, and, if skewed they can be intersecting or not. If lines
have !1  !2 D 0 they are parallel, otherwise, they are skewed.
The minimum distance between two lines is

d D !1 v 2 C !2 v 1

and is zero if they intersect.

For two lines ` 1 and ` 2 , the side operator is a permuted dot product
 
side ` 1 ; ` 2 D `10 `24 C `11 `25 C `12 `23 C `13 `22 C `14 `20 C `15 `21

which is zero if the lines intersect or are parallel, and is computed by the side
method. For Line3 objects L1 and L2, the operators L1 ^ L2 and L1 | L2 are true
if the lines are respectively intersecting or parallel.

Excurse C.1: Julius Plücker

Plücker (1801–1868) was a German mathematician and
physicist who made contributions to the study of cathode rays
and analytical geometry. He was born at Elberfeld and stud-
ied at Düsseldorf, Bonn, Heidelberg and Berlin, and went to
Paris in 1823 where he was influenced by the French ge-
ometry movement. In 1825, he returned to the University of
Bonn, was made professor of mathematics in 1828 (at age
27), and professor of physics in 1836. In 1858, he proposed
that the lines of the spectrum, discovered by his colleague
Heinrich Geissler (of Geissler tube fame), were characteris-
tic of the chemical substance which emitted them. In 1865,
he returned to geometry and invented what was known as line
geometry. He was the recipient of the Copley Medal from the
Royal Society in 1866, and is buried in the Alter Friedhof (Old
Cemetery) in Bonn.
728 C Geometry

We can transform a Plücker line between frames by

B 0
` D Ad. BA / A`

where the adjoint of the rigid-body motion is described by (D.2). This can be com-
puted by premultiplying a Line3 instance by an SE3 instance.

C.1.3 Planes

A plane is defined by a 4-vector D .a; b; c; d /> and is the set of all points x D
.x; y; z/> such that

ax C by C cz C d D 0

which can be written in point-normal form as

n> .x  p/ D 0

where n D .a; b; c/ is the normal to the plane, and p 2 R3 is a point in the plane.
A plane with the normal n and containing the point with coordinate vector p is
Plane.PointNormal(p, n) D .nx ; ny ; nz ; n p/> . A plane can also be defined by 3 points p1 , p 2 and p3 ,
where p i D .xi ; yi ; zi /
0 1
x1 y1 z1 1
Plane.ThreePoints(p) @x 2 y2 z2 1A D 0
x3 y3 z3 1

and solved for using the right null space of the leftmost term, or by two nonparallel
lines ` 1 and ` 2

Plane.TwoLines(l1, l2) D .!1  !2 ; v1 !2 /

or by a Plücker line .v; w/ and a point with coordinate vector p

Plane.LinePoint(l, p) D .!  p  v; v p/ :

A point can also be defined as the intersection point of three planes 1, 2 and 3

0 10 1 0 1
a1 b1 c1 x d1
Plane.intersection() @a 2 b2 c2 A@y A D @d2 A :
a3 b3 c3 z d3

If the left-hand matrix is singular, then two or more planes are parallel and have
either zero or infinitely many intersection points.
The Plücker line formed by the intersection of two planes 1 and 2 is

` D .n1  n2 ; d2 n1  d1 n2 / :

C.1.4 Ellipses and Ellipsoids

An ellipse belongs to the family of planar curves known as conics. The simplest
form of an ellipse, centered at .0; 0/, is defined implicitly by the points .x; y/ such
that
x2 y2
2
C 2 D1
a b
 729
C.1 Euclidean Geometry

and is shown in . Fig. C.2a. 7 This canonical ellipse is centered at the origin and A circle is a special case of an ellipse
has its major and minor axes aligned with the x- and y-axes. The radius in the where a D b D r and r is the radius.
x-direction is a and in the y-direction is b. The longer of the two radii is known as
the semi-major axis length, and the other is the semi-minor axis length.
We can write the ellipse in matrix quadratic form (B.3) as
! !
  1=a2 0 x
x y D1
0 1=b 2 y

and more compactly as

x> E x D 1 (C.2)

where x D .x; y/> and E is a symmetric matrix

!
˛ 
ED : (C.3)
 ˇ

Its determinant det.E/ D ˛ˇ   2 defines the type of conic

8
<> 0 ellipse
det.E/ D 0 parabola
:
< 0 hyperbola

An ellipse can therefore be represented by a positive-definite symmetric matrix E.

Conversely, any positive-definite symmetric 2  2 matrix, such as the inverse of an
inertia tensor or covariance matrix, has an equivalent ellipse.
Nonzero values of  change the orientation of the ellipse. The ellipse can be
arbitrarily centered at x c D .xc ; yc / by writing it in the form

.x  x c /> E.x  x c / D 1

which leads to the general ellipse shown in . Fig. C.2b.

Since E is symmetric, it can be diagonalized by (B.4)

E D XƒX>

where X is an orthogonal matrix comprising the eigenvectors of E, and the diagonal

elements of ƒ are the eigenvalues of E. The quadratic form (C.2) becomes

x > XƒX> x D 1
 > >  > 
X x ƒ X x D1
x 0> ƒx 0 D 1

minor axis major axis

semi-major axis length
semi-minor orientation
axis length
centroid
semi-major axis length
semi-minor
axis length

a b

. Fig. C.2 Ellipses. a Canonical ellipse centered at the origin and aligned with the x- and y-axes; b general form of ellipse
730 C Geometry

which is similar to (C.2) but with the ellipse defined by the diagonal matrix ƒ with
respect to the rotated coordinated frame x 0 D X> x. The major and minor ellipse
axes are aligned with the eigenvectors of E.
The radii of the ellipse are related to the inverse square root of the eigenvalues

1
ri D p (C.4)
i

so the major and minor radii of the ellipse, a and b respectively, are determined by
the smallest and largest eigenvalues respectively. The area of an ellipse is

ƒ D r1 r2 D p (C.5)
det.E/
Q
since det.E/ D i. The eccentricity is
p
a2  b 2
"D :
a
Alternatively, the ellipse can be represented in polynomial form by writing as
!
> ˛ 
.x  x c / .x  x c / D 1
 ˇ

and expanding to

e0 x 2 C e1 y 2 C e2 xy C e3 x C e4 y C e5 D 0

where e0 D ˛, e1 D ˇ, e2 D 2, e3 D 2.˛xc C yc /, e4 D 2.ˇyc C x c / and

e5 D ˛xc2 C ˇyc2 C 2xc yc  1. The ellipse has only five degrees of freedom, its
center coordinate and the three unique elements in E. For a nondegenerate ellipse
where e1 ¤ 0, we can rewrite the polynomial in normalized form

x 2 C 0 y 2 C 1 xy C 2 x C 3 y C 4 D 0 (C.6)

with five unique parameters i D ei C1 =e0 ; i D 0; :::; 4.

Consider the ellipse
!
1 1
x> xD1
1 2

which is represented in Python by

>>> E = np.array([[1, 1], [1, 2]])
array([[1, 1],
[1, 2]])

We can plot this by

>>> plot_ellipse(E)

which is shown in . Fig. C.3. The eigenvectors and eigenvalues of E are respec-
tively
>>> e, v = np.linalg.eig(E)
>>> e
array([ 0.382, 2.618])
>>> v
array([[ -0.8507, -0.5257],
[ 0.5257, -0.8507]])
 731
C.1 Euclidean Geometry

. Fig. C.3 Ellipse corresponding to a symmetric 2  2 matrix. The arrows indicate the major and minor
axes of the ellipse

The ellipse radii are

>>> r = 1 / np.sqrt(e)
array([ 1.618, 0.618])

If either radius is equal to zero the ellipse is degenerate and becomes a line. If both
radii are zero the ellipse is a point.
The eigenvectors are unit vectors in the major and minor axis directions 7 and Note that np.linalg.eig does not
we will scale them by the radii to yield radius vectors which we can plot sort the eigenvalues
>>> plot_arrow((0, 0), v[:,0]*r[0], color="r", width=0.02);
>>> plot_arrow((0, 0), v[:,1]*r[1], color="b", width=0.02);

The orientation of the ellipse is the angle of the major axis with respect to the
horizontal axis and is
vy
 D tan1
vx

where v D .vx ; vy / is the eigenvector corresponding to the smallest eigenvalue (the

largest radius). For our example, this is
>>> np.rad2deg(np.arctan2(v[1, 0], v[0, 0]))
148.3

in units of degrees, counter-clockwise from the x-axis.

The Toolbox function plot_ellipsoid will draw an ellipsoid and E is a 3  3
matrix.
C.1.4.1 Drawing an Ellipse
In order to draw an ellipse, we first define a set of points on the unit circle y D
.x; y/> such that

y >y D 1 : (C.7)

We rewrite (C.2) as
1 1
x> E 2 E 2 x D 1 (C.8)
1
where E 2 is the matrix square root, and equating (C.7) and (C.8), we can write
1 1
x> E 2 E 2 x D y >y :
732 C Geometry

which leads to
1
y D E2 x

which we can rearrange as

1
x D E 2 y

1
that transforms a point on the unit circle to a point on an ellipse, and E 2 is the
inverse of the matrix square root. If the ellipse is centered at x c , rather than the
origin, we can perform a change of coordinates

.x  x c /> E 2 E 2 .x  x c / D 1
1 1

from which we write the transformation as

1
x D E 2 y C x c

Drawing an ellipsoid is tackled in an analogous fashion.

Continuing the code example above
>>> E = np.array([[1, 1], [1, 2]]);

We define a set of points on the unit circle

>>> th = np.linspace(0, 2*pi, 50);
>>> y = np.vstack([np.cos(th), np.sin(th)]);

which we transform to points on the perimeter of the ellipse

>>> x = linalg.sqrtm(np.linalg.inv(E)) @ y;
>>> plt.plot(x[0, :], x[1, :], '.');

which is encapsulated in the Toolbox function

>>> plot_ellipse(E, centre=[0, 0])

In many cases we wish to plot an ellipse given by A1 . Computing the inverse of
A is inefficient because plot_ellipse will invert it again, so in this case we write
plot_ellipse(A, inverted=True).

C.1.4.2 Fitting an Ellipse to Data

A common problem is to find the equation of an ellipse that best fits a set of points
that lie within the ellipse boundary, or that lie on the ellipse boundary.

From a Set of Interior Points

To fit an ellipse to a set of points within the ellipse boundary, we find the ellipse
that has the same mass properties as the set of points. From the set of N points
x i D .xi ; yi /> , we can compute the moments

X
N 1 X
N 1
m00 D N; m10 D xi ; m01 D yi :
i D0 i D0

The center of the ellipse is taken to be the centroid of the set of points

>
m10 m01
x c D .xc ; yc /> D ;
m00 m00
 733
C.1 Euclidean Geometry

which allows us to compute the central second moments

X
N 1
20 D .xi  xc /2
i D0

X
N 1
02 D .yi  yc /2
i D0

X
N 1
11 D .xi  xc /.yi  yc / :
i D0

The inertia tensor for a general ellipse is the symmetric matrix

!
20 11
JD
11 02

where the diagonal terms are the moments of inertia and the off-diagonal terms are
the products of inertia. Inertia can be computed more directly as the summation of
N rank-1 matrices

X
N 1
JD .x i  x c /.x i  x c /> :
i D0

The inertia tensor and its equivalent ellipse are inversely related by
m00 1
ED J :
4
To demonstrate this, we can create a set of points that lie within the ellipse used in
the example above
>>> rng = np.random.default_rng(0);
>>> # create 200 random points inside the ellipse
>>> x = [];
>>> while len(x) < 200:
... p = rng.uniform(low=-2, high=2, size=(2,1))
... if np.linalg.norm(p.T @ E @ p) <= 1:
... x.append(p)
>>> x = np.hstack(x); # create 2 x 50 array
>>> plt.plot(x[0, :], x[1, :], "k."); # plot them
>>>
>>> # compute the moments
>>> m00 = mpq_point(x, 0, 0);
>>> m10 = mpq_point(x, 1, 0);
>>> m01 = mpq_point(x, 0, 1);
>>> xc = np.c_[m10, m01] / m00;
>>>
>>> # compute the central second moments
>>> x0 = x - xc.T;
>>> u20 = mpq_point(x0, 2, 0);
>>> u02 = mpq_point(x0, 0, 2);
>>> u11 = mpq_point(x0, 1, 1);
>>>
>>> # compute inertia tensor and ellipse matrix
>>> J = np.array([[u20, u11], [u11, u02]]);
>>> E_est = m00 / 4 * np.linalg.inv(J);

which results in an estimate

>>> E_est
array([[ 1.053, 1.038],
[ 1.038, 1.98]])
734 C Geometry

. Fig. C.4 200 random data points with a fitted ellipse

that is similar to the original value of E. The point data is shown in . Fig. C.4. We
can overlay the estimated ellipse on the point data
>>> plot_ellipse(E_est, "r")

and the result is shown in red in . Fig. C.4.

From a Set of Perimeter Points

Given a set of points .xi ; yi / that lie on the perimeter of an ellipse, we use the
polynomial form of the ellipse (C.6) for each point. We write this in matrix form
with one row per point
0 10 1 0 1
y02 x0 y0 x0 y0 1 0 x02
B CB C B C
B 1C B 1C B 2 C
B y12 x1 y2 x1 y1 CB C B x1 C
B :: :: :: :: :: C B2 C D B : C
B :C B C B : C
@ : : : : A@3 A @ : A
yN2 1 xN 1 yN 1 xN 1 yN 1 1 4 xN2 1

and for N 5 we can solve for the ellipse parameter vector using least squares.

C.2 Homogeneous Coordinates

A point in homogeneous coordinates, or the projective space P n , is represented by

a coordinate vector xQ D .xQ 0 ; xQ 1 ; : : : ; xQ n / and the tilde is used to indicate that the
quantity is homogeneous. The Euclidean coordinates are related to the projective
or homogeneous coordinates by

xQ i
xi D ; i D 0; : : : ; n  1
xQ nC1

Conversely, a homogeneous coordinate vector can be constructed from a Euclidean

coordinate vector by

xQ D .x0 ; x1 ; : : : ; xn1 ; 1/ :

The extra degree of freedom offered by projective coordinates has several ad-
vantages. It allows points and lines at infinity, known as ideal points and lines, to be
 735
C.2 Homogeneous Coordinates

ay
er
tiv
jec
pro
1

or
ect
ev
tiv
jec
pro
Euclidean plane
projective space

. Fig. C.5 A point p on the Euclidean plane is described by a coordinate vector p 2 R2 which is
equivalent to the three-dimensional vector in the projective space pQ 2 P 2

represented using only finite numbers. It also means that scale is unimportant, that
0
is, xQ and xQ D xQ both represent the same Euclidean point for all ¤ 0. We ex-
0
press this as xQ ' xQ . We can apply a rigid-body transformation to points xQ 2 P n by
multiplying the homogeneous coordinate by an n  n homogeneous transformation
matrix.
Homogeneous vectors are important in computer vision when we consider
points and lines that exist in a plane, for example, a camera’s image plane.
We can also consider that the homogeneous form represents a ray in projec-
tive space as shown in . Fig. C.5. Any point on the projective ray is equivalent
to p and this relationship between points and rays is at the core of the pro-
jective transformation. You can experiment with this by running the notebook
examples/homogeneous-coords.ipynb.

C.2.1 Two Dimensions

C.2.1.1 Points and Lines

In two dimensions, there is a duality between points and lines. In P 2 , a line is
represented by a vector Q̀ D .`0 ; `1 ; `2 /> , not all zero, and the equation of the line
is the set of all points xQ 2 P 2 such that

Q̀ > xQ D 0

which is the point equation of a line. This expands to `0 x C`1 y C`2 D 0 and can be
manipulated into the more familiar representation of a line y D  ``01 x  ``21 . Note that
this form can represent a vertical line which the familiar form y D mx C c cannot.
The nonhomogeneous vector .`0 ; `1 /> is normal to the line, 7 and .`1 ; `0 /> is Hence, this line representation is
parallel to the line. referred to as normal form.
A point is defined by the intersection of two lines. If we write the point equa-
> >
tions for two lines Q̀ 1 xQ D 0 and Q̀ 2 xQ D 0, their intersection is the point with
homogeneous coordinates

pQ D Q̀ 1  Q̀ 2

and is known as the line equation of a point. Similarly, a line passing through two
points pQ 1 ; pQ 2 2 P 2 , said to be joining the points, is given by the cross-product

Q̀ 12 D pQ 1  pQ 2 :
736 C Geometry

Consider the case of two parallel lines at 45ı to the horizontal axis
>>> l1 = [1, -1, 0];
>>> l2 = [1, -1, -1];

which we can plot

>>> plot_homline(l1, "b");
>>> plot_homline(l2, "r");

The intersection point of these parallel lines is

>>> np.cross(l1, l2)
array([1, 1, 0])

This is an ideal point since the third coordinate is zero – the equivalent Euclidean
point would be at infinity. Projective coordinates allow points and lines at infinity
to be simply represented and manipulated without special logic.
The distance from a line Q̀ to a point pQ is

Q̀ > pQ
d D q : (C.9)
pQ2 `Q20 C `Q21

C.2.1.2 Conics
Conic sections are an important family of planar curves that includes circles, el-
lipses, parabolas and hyperbolas. They can be described generally as the set of
points xQ 2 P 2 such that

xQ > xQ D 0

where  is a symmetric matrix

0 1
a b=2 d=2
 D @ b=2 c e=2 A : (C.10)
d=2 e=2 f

The determinant of the top-left submatrix indicates the type of conic: positive for
an ellipse, zero for a parabola, and negative for a hyperbola.

C.2.2 Three Dimensions

In three dimensions, there is a duality between points and planes.

C.2.2.1 Lines
Q qQ 2 P 3 in homogeneous form, the line that joins them is defined
For two points p;
by a 4  4 skew-symmetric matrix

L D qQ pQ >  pQ qQ >
0 1
0 v3 v2 !1
Bv3 0 v1 !2 C
DB@ v2 v1
C (C.11)
0 !3 A
!1 !2 !3 0

whose six unique elements comprise the Plücker coordinate representation of the
line. This matrix is rank 2 and the determinant is a quadratic in the Plücker co-
ordinates – a 4-dimensional quadric hypersurface known as the Klein quadric.
All points that lie on this manifold are valid lines. Many of the relationships in
 737
C.3 Geometric Transformations

7 App. C.1.2.2 (between lines and points and planes) can be expressed in terms of
this matrix. This matrix is returned by the skew method of the Line3 class.
For a perspective camera with a camera matrix C 2 R34 , the 3-dimensional
Plücker line represented as a 4  4 skew-symmetric matrix L is projected onto the
image plane as

Q̀ D CLC> 2 P 2

which is a homogeneous 2-dimensional line. This is computed by the project_line

method of the CentralCamera class.

C.2.2.2 Planes
A plane Q 2 P 3 is described by the set of points xQ 2 P 3 such that Q > xQ D 0. A
plane can be defined by a line L 2 R44 and a homogeneous point pQ

Q D LpQ Plane.LinePoint()

or three points
0 >1
pQ 1
B >C
@pQ 2 A Q D 0 Plane.ThreePoints()

pQ >
3

and the solution is found from the right-null space of the matrix.
The intersection, or incidence, of three planes is the dual
0 1
Q>
1
B >C
@ Q 2 ApQ D 0 Plane.intersection()

Q>
3

and is an ideal point, zero last component, if the planes do not intersect at a point.

C.2.2.3 Quadrics
Quadrics, short for quadratic surfaces, are a rich family of 3-dimensional sur-
faces. There are 17 standard types including spheres, ellipsoids, hyperboloids,
paraboloids, cylinders and cones, all described by the set of points xQ 2 P 3 such
that

xQ > QxQ D 0

and Q 2 R44 is symmetric.

For a perspective camera with a camera matrix C 2 R34 , the outline of the
3-dimensional quadric is projected to the image plane by

 D CQ C>

where  is given by (C.10), and . / represents the adjugate operation, see

7 App. B.1.

C.3 Geometric Transformations

A linear transformation is

y D Ax (C.12)
738 C Geometry

while an affine transformation

y D Ax C b (C.13)

comprises a linear transformation and a change of origin. Examples of affine trans-

Scaling about an arbitrary point. formations include translation, scaling, homothety, 9 reflection, rotation, shearing,
and any arbitrary composition of these. Every linear transformation is affine, but
not every affine transformation is linear.
In homogeneous coordinates, we can write (C.13) as
!
A b
yQ D Hx;
Q where H D
0 1

and the transformation operates on a point with homogeneous coordinates x. Q

Projective space is a generalization of affine space which, in turn, is a general-
ization of Euclidean space. An affine space has no distinguished point that serves
as an origin and hence no vector can be uniquely associated to a point. An affine
space has only displacement vectors between two points in the space. Subtracting
two points results in a displacement vector, and adding a displacement vector to a
point results in a new point. If a displacement vector is defined as the difference
between two homogeneous points pQ and q, Q then the difference pQ  qQ is a 4-vector
whose last element will be zero, distinguishing a point from a displacement vector.
In two dimensions, the most general transformation is a projective transforma-
tion, also known as a collineation
0 1
h0;0 h0;1 h0;2
H D @h1;0 h1;1 h1;2 A
h2;1 h2;1 1

which is unique up to scale and one element has been normalized to one. It has
8 degrees of freedom.
The affine transformation is a subset where the elements of the last row are
fixed
0 1
a0;0 a0;1 tx
H D @a1;0 a1;1 ty A
0 0 1

and has 6 degrees of freedom, two of which are the translation .tx ; ty /.
The similarity transformation is a more restrictive subset
! 0sr sr0;1 tx
1
0;0
sR t
HD D @sr1;0 sr1;1 ty A
0 1
0 0 1

where R 2 SO.2/ resulting in only 4 degrees of freedom, and s < 0 causes a reflec-
tion. Similarity transformations, without reflection, are sometimes referred to as a
Procrustes transformation.
Finally, the Euclidean or rigid-body transformation
! 0r r0;1 tx
1
0;0
R t
HD D @r1;0 r1;1 ty A 2 SE.2/
0 1
0 0 1

is the most restrictive and has only 3 degrees of freedom. Some graphical examples
of the effect of the various transformations on a square are shown in . Fig. C.6.
 739
C.3 Geometric Transformations

. Fig. C.6 A 2-dimensional dark gray square with various transformations applied: from the most
limited (Euclidean) to the most general (projective)

. Table C.1 For various planar transformation families, the possible geometric transformations
and the geometric properties which are preserved are listed

Euclidean Similarity Affine Projective

Geometric transformation
Rotation X X X X

Translation X X X X

Reflection X X X

Uniform scaling X X X

Nonuniform scaling X X

Shear X X

Perspective projection X

Euclidean Similarity Affine Projective

Preserved geometric properties (invariants)

Length X

Angle X X

Ratio of lengths X X

Parallelism X X X

Incidence X X X X

Cross ratio X X X X

The possible geometric transformations for each type of transformation are sum-
marized in . Table C.1 along with the geometric properties which are unchanged,
or invariant, under that transformation. We see that while Euclidean is most restric-
tive in terms of the geometric transformations it can perform, it is able to preserve
important properties such as length and angle.
740 D Lie Groups and Algebras

D Lie Groups and Algebras

We cannot go very far in the study of rotations or rigid-body motion without com-
ing across the terms Lie groups, Lie algebras or Lie brackets – all named in honor
of the Norwegian mathematician Sophus Lie. Rotations and rigid-body motion in
two and three dimensions can be represented by matrices that have special struc-
ture, they form Lie groups, and they have Lie algebras.
We will start simply by considering the set of all real 2  2 matrices A 2 R22
!
a0;0 a0;1
AD
a1;0 a1;1

which we could write as a linear combination of basis matrices

! ! ! !
1 0 0 1 0 0 0 0
A D a0;0 C a0;1 C a1;0 C a1;1
0 0 0 0 1 0 0 1
where each basis matrix represents a direction in a 4-dimensional space of 2 
2 matrices. That is, the four axes of this space are parallel with each of these
basis matrices. Any 2  2 matrix can be represented by a point in this space – this
particular matrix is a point with the coordinates .a0;0 ; a0;1 ; a1;0 ; a1;1 /.
All proper rotation matrices, those belonging to SO(2), are a subset of points
within the space of all 2  2 matrices. For this example, the points lie in a 1-
dimensional subset, a closed curve, in the 4-dimensional space. This is an instance
of a manifold, a lower-dimensional smooth surface embedded within a space.
The notion of a curve in the 4-dimensional space makes sense when we consider
that the SO(2) rotation matrix
!
cos  sin 
AD
 sin  cos 

has only one free parameter, and varying that parameter moves the point along the
manifold.
Invoking mathematical formalism, we say that rotations SO(2) and SO(3), and
rigid-body motions SE(2) and SE(3) are matrix Lie groups and this has two impli-
cations. Firstly, they are an algebraic group, a mathematical structure comprising

Excurse D.2: Sophus Lie

Lie, pronounced lee, (1842–1899) was a Norwegian math-
ematician who obtained his Ph.D. from the University of
Christiania in Oslo in 1871. He spent time in Berlin work-
ing with Felix Klein, and later contributed to Klein’s Erlangen
program to characterize geometries based on group theory and
projective geometry. On a visit to Milan during the Franco-
Prussian war, he was arrested as a German spy and spent one
month in prison. He is best known for his discovery that con-
tinuous transformation groups (now called Lie groups) can be
understood by linearizing them and studying their generating
vector spaces. He is buried in the Vår Frelsers gravlund in
Oslo. (Image by Ludwik Szacinski)
 741
D Lie Groups and Algebras

elements and a single operator. In simple terms, a group G has the following prop-
erties:
1. If g1 and g2 are elements of the group, that is, g1 ; g2 2 G, then the result of
the group’s operator ˘ is also an element of the group: g1 ˘g2 2 G. In general, In this book’s notation, ˚ is the group
groups are not commutative, so g1 ˘g2 ¤ g2 ˘g1 . For rotations and rigid-body operator for relative pose.
motions, the group operator ˘ represents composition. 7
2. The group operator is associative, that is, .g1 ˘g2 /˘g3 D g1 ˘.g2 ˘g3 /. In this book’s notation, the identity for
3. There is an identity element I 2 G such, for every g 2 G, g˘I D I ˘g D g. 7 relative pose (implying null motion) is
4. For every g 2 G, there is a unique inverse h 2 G such that g˘h D h˘g D I . 7 denoted by ¿ so we can say that
 ˚ ¿ D ¿ ˚  D .
The second implication of being a Lie group is that there is a smooth (differen-
tiable) manifold structure. At any point on the manifold, we can construct tangent In this book’s notation, we use the
vectors. The set of all tangent vectors at that point form a vector space – the tan- operator  to form the inverse of a
gent space. This is the multidimensional equivalent to a tangent line on a curve, or relative pose.
a tangent plane on a solid. We can think of this as the set of all possible derivatives
of the manifold at that point.
The tangent space at the identity is described by the Lie algebra of the group,
and the basis directions of the tangent space are called the generators of the group.
Points in this tangent space map to elements of the group via the exponential func-
tion. If g is the Lie algebra for group G, then
eX 2 G; 8X 2 g
where the elements of g and G are matrices of the same size and each of them has
a specific structure.
The surface of a sphere is a manifold in a 3-dimensional space and at any point
on that surface we can create a tangent vector. In fact, we can create an infinite
number of them and they lie within a plane which is a 2-dimensional vector space
– the tangent space. We can choose a set of basis directions and establish a 2-
dimensional coordinate system and we can map points on the plane to points on
the sphere’s surface.
Now, consider an arbitrary real 3  3 matrix A 2 R33
0 1
a0;0 a0;1 a0;2
A D @a1;0 a1;1 a1;2 A
a2;0 a2;1 a2;2
which we could write as a linear combination of basis matrices
0 1 0 1 0 1
1 0 0 0 1 0 0 0 0
A D a0;0 @0 0 0A C a0;1 @0 0 0A C C a2;2 @0 0 0A
0 0 0 0 0 0 0 0 1
where each basis matrix represents a direction in a 9-dimensional space of 3 
3 matrices. Every possible 3  3 matrix is represented by a point in this space.
Not all matrices in this space are proper rotation matrices belonging to SO(3),
but those that do will lie on a manifold since SO(3) is a Lie group. The null rotation,
represented by the identity matrix, is one point in this space. At that point, we can
construct a tangent space which has only 3 dimensions. Every point in the tangent
space can be expressed as a linear combination of basis matrices
0 1 0 1 0 1
0 0 0 0 0 1 0 1 0
 D !1 @0 0 1A C!2 @ 0 0 0A C!3 @1 0 0A (D.1)
0 1 0 1 0 0 0 0 0
„ ƒ‚ … „ ƒ‚ … „ ƒ‚ …
G1 G2 G3
The equivalent algebra is denoted
which is the Lie algebra of the SO(3) group. The bases of this space: G1 , G2 and using lowercase letters and is a set of
G3 are called the generators of SO(3) and belong to so(3). 7 matrices.
742 D Lie Groups and Algebras

Equation (D.1) can be written as a skew-symmetric matrix

0 1
0 !3 !2
 D Œ! D @ !3 0 !1 A 2 so.3/
!2 !1 0

parameterized by the vector ! D .!1 ; !2 ; !3 / 2 R3 which is known as the Euler

vector or exponential coordinates. This reflects the 3 degrees of freedom of the
SO(3) group embedded in the space of all 3  3 matrices. The 3DOF is consistent
with our intuition about rotations in 3D space and also Euler’s rotation theorem.
Mapping between vectors and skew-symmetric matrices is frequently required
and the following shorthand notation will be used

Π W R 7! so.2/; R3 7! so.3/;
_ . /W so.2/ 7! R; so.3/ 7! R3 :

The first mapping is performed by the Toolbox function skew and the second by
vex(which is named after the _ operator).
The exponential of any matrix in so(3) is a valid member of SO(3)
O 
O D eΠ!
R. !/ 2 SO.3/

and an efficient closed-form solution is given by Rodrigues’ rotation formula

O D 1 C sin Œ!
R. !/ O 2
O  C .1  cos /Œ!

Finally, consider an arbitrary real 4  4 matrix A 2 R44

0 1
a0;0 a0;1 a0;2 a0;3
Ba1;0 a1;1 a1;2 a1;3 C
ADB @a2;0 a2;1 a2;2 a2;3 A
C

a3;0 a3;1 a3;2 a3;3

which we could write as a linear combination of basis matrices

0 1 0 1 0 1
1 0 0 0 0 1 0 0 0 0 0 0
B0 0 0 0 C B0 0 0 0 C B0 0 0 0 C
A D a0;0 B C B
@0 0 0 0A C a0;1 @0 0 0 0A C
C C a3;3 B
@0 0 0 0 A
C

0 0 0 0 0 0 0 0 0 0 0 1

where each basis matrix represents a direction in a 16-dimensional space of all

possible 4  4 matrices. Every 4  4 matrix is represented by a point in this space.
Not all matrices in this space are proper rigid-body transformation matrices
belonging to SE(3), but those that do lie on a smooth manifold. The null motion
(zero rotation and translation), which is represented by the identity matrix, is one
point in this space. At that point, we can construct a tangent space, which has
6 dimensions in this case, and points in the tangent space can be expressed as a
linear combination of basis matrices
0 1 0 1 0 1
0 0 0 0 0 0 1 0 0 1 0 0
B0 0 1 0C B 0 0 0 0C B1 0 0 0C
† D !1 B C B
@0 1 0 0A C !2 @1 0 0 0A C !3 @0 0 0 0A
C B C

0 0 0 0 0
0 0 0 0 0 0 0
0 1 0 1 0 1
0 0 0 1 0 0 0 0 0 0 0 0
B0 0 0 0C B0 0 0 1 C B0 0 0 0C
C v1 B
@0
C C v2 B C B
@0 0 0 0A C v3 @0
C
0 0 0A 0 0 1A
0 0 0 0 0 0 0 0 0 0 0 0
 743
D Lie Groups and Algebras

and these generator matrices belong to the Lie algebra of the group SE(3) which is
enoted by se(3). This can be written in general form as
0 1
0 !3 !2 v1
B C
B !3 0 !1 v2 C
† D ŒS  D B C 2 se.3/
@ !2 !1 0 v3 A
0 0 0 0

which is an augmented skew-symmetric matrix parameterized by S D .v; !/ 2 R6

which is referred to as a twist and has physical interpretation in terms of a screw
axis direction and position. The sparse matrix structure and this concise parameter-
ization reflects the 6 degrees of freedom of the SE(3) group embedded in the space
of all 4  4 matrices. We extend our earlier shorthand notation

ΠW R3 7! se.2/; R6 7! se.3/;
_. /W se.2/ 7! R3 ; se.3/ 7! R6 :

We can use these operators to convert between a twist representation which is a 6-

vector and a Lie algebra representation which is a 44 augmented skew-symmetric
matrix. We convert the Lie algebra to the Lie group representation using

O
T. SO / D eΠS  2 SE.3/

or the inverse using the matrix logarithm. The exponential and the logarithm each
have an efficient closed-form solution given by trexp and trlog.

Transforming a Twist – the Adjoint Representation

We have seen in 7 Sect. 2.4.7 that rigid-body motions can be described by a twist
which represents motion in terms of a screw axis direction and position. For ex-
ample, in . Fig. D.1, the twist AS can be used to transform points on the body. If
the screw is rigidly attached to the body which undergoes some rigid-body motion
A
B the new twist is
B A
B
S D Ad A S

where

A  A !
  A
RB t B  RB
Ad AB D A
2 R66 (D.2)
0 RB

. Fig. D.1 Points in the body (gray cloud) can be transformed by the twist AS . If the body and the
screw axis undergo a rigid-body transformation AB , the new twist is BS
744 D Lie Groups and Algebras

SE3.Ad
adjoint
logm
Twist()
expm Twist.T
SE (3)
Twist.ad
SE3.log SE3.exp

log
adjoint
vexa,Twist()
se (3)
skewa,Twist.se
6x1 vectors: 6x6 matrices 4x4 matrices 4x1 homogenous vectors
- twist - adjoint - SE(3), homogeneous -points
- velocity twist - log adjoint - se(3)
- wrench
- Plücker coordinates

. Fig. D.2 The menagerie of SE.3/ related quantities. Matrix values are coded as: 0 (black), 1 (white), other values (gray). Transformations between
types are indicated by blue arrows with the relevant class plus method name. Operations are indicated by red arrows: the tail-end object operates on the
head-end object and results in another object of the head-end type

is the adjoint representation of the rigid-body motion. Alternatively, we can write

Ad.eŒS  / D ead.S /

where ad.S / is the logarithm of the adjoint and defined in terms of the twist pa-
rameters as
!
Œ! Œv
ad.S / D 2 R66 :
0 Œ!

The relationship between the various mathematical objects discussed are shown
in . Fig. D.2.
 745
E Linearization, Jacobians and Hessians

E Linearization, Jacobians and Hessians

In robotics and computer vision, the equations we encounter are often nonlin-
ear. To apply familiar and powerful analytic techniques, we must work with lin-
ear or quadratic approximations to these equations. The principle is illustrated in
. Fig. E.1 for the 1-dimensional case, and the analytical approximations shown in
red are made at x D x0 . The approximation equals the nonlinear function at x0 but
is increasingly inaccurate as we move away from that point. We call this a local
approximation since it is valid in a region local to x0 – the size of the valid region
depends on the severity of the nonlinearity. This approach can be extended to an
arbitrary number of dimensions.

E.1 Scalar Function of a Scalar

The function f : R 7! R can be expressed as a Taylor series

df 1 d2 f
f .x0 C / D f .x0 / C C 2
C
dx 2 dx 2
which we truncate to form a first-order or linear approximation

f 0 . /  f .x0 / C J.x0 /

or a second-order approximation

1
f 0 . /  f .x0 / C J.x0 / C H.x0 / 2
2
where 2 R is an infinitesimal change in x relative to the linearization point x0 ,
and the first and second derivatives are given by
ˇ ˇ
df ˇˇ d2 f ˇˇ
J.x0 / D ; H.x0 / D
dx ˇx0 dx 2 ˇx0

respectively.

a b

. Fig. E.1 The nonlinear function f .x/ is approximated at the point x D x0 by a the red line – a linear
or first-order approximation, b the red parabola – a second-order approximation. At the linearization
point, both curves are equal and tangent to the function while for b the second derivatives also match
746 E Linearization, Jacobians and Hessians

Excurse E.3: Ludwig Otto Hesse

Hesse (1811–1874) was a German mathematician, born in
Königsberg, Prussia, who studied under Jacobi and Bessel at
the University of Königsberg. He taught at Königsberg, Halle,
Heidelberg and finally at the newly established Polytechnic
School in Munich. In 1869, he joined the Bavarian Academy
of Sciences. He died in Munich, but was buried in Heidelberg
which he considered his second home.

E.2 Scalar Function of a Vector

The scalar field f .x/W Rn 7! R can be expressed as a Taylor series

1 >
f .x 0 C / D f .x 0 / C J.x 0 / C H.x 0 / C
2
which we can truncate to form a first-order or linear approximation

f 0 . /  f .x 0 / C J.x 0 /

or a second-order approximation
1
f 0 . /  f .x 0 / C J.x 0 / C >
H.x 0 /
2
where 2 Rn is an infinitesimal change in x 2 Rn relative to the linearization
point x 0 , J 2 R1n is the vector version of the first derivative, and H 2 Rnn is the
Hessian – the matrix version of the second derivative.
The derivative of the function f . / with respect to the vector x is
0 @f 1
@x0
B C
B @f
C
B @x1 C
J.x/ D rf .x/ D B :: C 2 Rn
B C
@ : A
@f
@xn1

and is a column vector that points in the direction at which the function f .x/ has
maximal increase. It is often written as rx f to make explicit that the differentiation
is with respect to x.
The Hessian is an n  n symmetric matrix of second derivatives
0 2 2 2
1
@ f @ f @ f
B @x02 @x0 @x1 @x0 @xn1
C
B @2 f @2 f @2 f C
B C
H.x/ D B C 2 Rnn :
@x0 @x1 @x12 @x1 @xn1
B :: :: :: :: C
B : : : : C
@ A
@2 f @2 f @2 f
@x0 @xn1 @x1 @xn1 2
@xn1
 747
E.3 Vector Function of a Vector

The function is at a critical point when kJ.x/k D 0. If H.x/ is positive-definite

then the function is at a local minimum, if negative-definite then a local maximum,
otherwise the result is inconclusive.
For functions which are quadratic in x, as is the case for least-squares problems,
it can be shown that the Hessian is
X
m1
@2 fi
H.x/ D J>.x/ J.x/ C fi .x/  J>.x/ J.x/
i D0
@x2

which is frequently approximated by just the first term and this is key to Gauss-
Newton least-squares optimization discussed in 7 App. F.2.3.

E.3 Vector Function of a Vector

The vector field f .x): Rn 7! Rm can also be written as

0 1
f0 .x/
B f1 .x/ C
B C
f .x/ D B :: C 2 Rm
@ : A
fm1 .x/
where fi W Rm ! R; i D 0; :::; m1. The derivative of f with respect to the vector x
can be expressed in matrix form as a Jacobian matrix
0 @f @f
1
0 0

B @x0 @xn1
C
J.x/ D B
@
::
:
::
:
::
:
C 2 Rmn
A
@fm1 @fm1
@x0 @xn1

which can also be written as

0 1
rf0>
B C
B rf1> C
B C
J.x/ D B :: C
B C
@ : A
>
rfm1
This derivative is also known as the tangent map of f , denoted by Tf , or the
differential of f denoted by Df . To make explicit that the differentiation is with
respect to x this can be denoted as Jx , Tx f , Dx f or even @f =@x.
The Hessian in this case is a rank-3
 tensor which we can think of as a list, of
length m, of n  n Hessians H D H0 .x/; : : : ; Hm1 .x/ . In Python this could be
represented by an n  n  m array.

E.4 Deriving Jacobians

Jacobians of functions are required for many optimization algorithms, for example
pose-graph optimization, as well as for the extended Kalman filter. They can be
computed either numerically or symbolically.
Consider (6.9) for the range and bearing angle of a landmark given the pose
of the vehicle and the position of the landmark. We can express this as a lambda
function
>>> zrange = lambda xi, xv, w: np.array([
... np.linalg.norm(xi - xv[:2]) + w[0],
... np.arctan((xi[1] - xv[1]) / (xi[0] - xv[0]))
... -xv[2] + w[1]]);
748 E Linearization, Jacobians and Hessians

To estimate the Jacobian Hxv D @h=@x v for x v D .1; 2; 3 / and x i D .10; 8/,
we can compute a first-order numerical difference
>>> xv = np.r_[1, 2, pi/3]; xi = np.r_[10, 8]; w = np.r_[0, 0];
>>> h0 = zrange(xi, xv, w)
array([ 10.82, -0.4592])
>>> d = 0.001;
>>> J = np.column_stack([
... zrange(xi, xv + [d, 0, 0], w) - h0,
... zrange(xi, xv + [0, d, 0], w) - h0,
... zrange(xi, xv + [0, 0, d], w) - h0
... ]) / d
array([[ -0.832, -0.5547, 0],
[ 0.05129, -0.07693, -1]])

which shares the characteristic last column with the Jacobian shown in (6.14). Note
that in computing this Jacobian we have set the measurement noise w to zero. The
principal difficulty with this approach is choosing d, the difference used to compute
the finite-difference approximation to the derivative. Too large and the results will
be quite inaccurate if the function is nonlinear, too small and numerical problems
will lead to reduced accuracy.
The Toolbox provides a convenience function to perform this
>>> numjac(lambda x: zrange(xi, x, w), xv)
array([[ -0.8321, -0.5547, 0],
[ 0.05128, -0.07692, -1]])

where the arguments are a scalar function of a vector and the value of the vector
about which to linearize.
Alternatively, we can perform the differentiation symbolically. This particular
function is relatively simple and the derivatives can be determined easily by hand
using differential calculus. The numerical derivative can be used as a quick check
for correctness. To avoid the possibility of error, or for more complex functions, we
can perform the symbolic differentiation using any of a large number of computer
algebra packages. Using SymPy we define some symbolic column vectors for x i
and x v which are 2D arrays
>>> from sympy import Matrix, MatrixSymbol, sqrt, atan, simplify,
... pycode
>>> xi = MatrixSymbol("xi", 2, 1)
xi
>>> xv = MatrixSymbol("xv", 3, 1)
xv
>>> w = Matrix([0, 0])
Matrix([
[0],
[0]])

Now we have to rewrite the function in a SymPy friendly way, removing NumPy
NumPy functions will not accept functions and replacing them with equivalent SymPy functions 9
symbolic variables.
>>> zrange = lambda xi, xv, w: Matrix([
... sqrt((xi[0] - xv[0])**2 + (xi[1] - xv[1])**2) + w[0],
... atan((xi[1] - xv[1]) / (xi[0] - xv[0]))
... -xv[2] + w[1]]);
>>> z = zrange(xi, xv, w)
Matrix([[sqrt((xi[0, 0] - xv[0, 0])**2 + (xi[1, 0] - xv[1, 0])**2)],
[atan((xi[1, 0] - xv[1, 0])/(xi[0, 0] - xv[0, 0]))
- xv[2, 0]]])

which is simply (6.9) in SymPy symbolic form and using SymPy Matrix rather
than NumPy array. The result is less compact because SymPy supports only 2D
 749
E.4 Deriving Jacobians

arrays, so we have to provide two indices to reference an element of a vector. The

Jacobian is computed by
>>> J = z.jacobian(xv)
Matrix([
[(-xi[0, 0] + xv[0, 0])/sqrt((xi[0, 0] - xv[0, 0])**2
+ (xi[1, 0] - xv[1, 0])**2),
(-xi[1, 0] + xv[1, 0])/sqrt((xi[0, 0] - xv[0, 0])**2
+ (xi[1, 0] - xv[1, 0])**2),
0],
[(xi[1, 0] - xv[1, 0])/((xi[0, 0] - xv[0, 0])**2
+ (xi[1, 0] - xv[1, 0])**2),
(-xi[0, 0] + xv[0, 0])/((xi[0, 0] - xv[0, 0])**2
+ (xi[1, 0] - xv[1, 0])**2),
-1]
])

which has the required shape

>>> J.shape
(2, 3)

and the characteristic last column. We could cut and paste this code into our pro-
gram or automatically create a Python callable function using pycode.
Another approach is automatic differentiation (AD) of code. The package
ADOL-C is an open-source tool that can differentiate C and C++ programs, that
is, given a function written in C, it will return a Jacobian or higher-order derivative
function written in C. It is available at 7 https://github.com/coin-or/ADOL-C. For
Python code, there is an open-source tool called Tangent available at 7 https://
github.com/google/tangent.
750 F Solving Systems of Equations

F Solving Systems of Equations

Solving systems of linear and nonlinear equations, particularly over-constrained

systems, is a common problem in robotics and computer vision.

F.1 Linear Problems

F.1.1 Nonhomogeneous Systems

These are equations of the form

Ax D b
where we wish to solve for the unknown vector x 2 Rn where A 2 Rmn and b 2 Rm
are known constants.
If n D m then A is square, and if A is non-singular then the solution is
x D A1 b :
In practice, we often encounter systems where m > n, that is, there are more equa-
tions than unknowns. In general, there will not be an exact solution, but we can
attempt to find the best solution, in a least-squares sense, which is
x  D arg min jjAx  bjj :
x

That solution is given by

 1
x  D A> A A> b D AC b
which is known as the pseudoinverse or more formally the left-generalized inverse.
Using SVD where A D U†V> , this is
x D V† 1 U> b

where † 1 is simply the element-wise inverse of the diagonal elements of †.

If the matrix is singular, or the system is under constrained n < m, then there
are infinitely many solutions. We can again use the SVD approach
x D V† 1 U> b

where this time † 1 is the element-wise inverse of the nonzero diagonal elements
of †, all other zeros are left in place.
All these problems can be solved using SciPy
>>> x = linalg.spsolve(A, b)

F.1.2 Homogeneous Systems

These are equations of the form

Ax D 0 (F.1)
and always have the trivial solution x D 0. If A is square and non-singular, that
is the only solution. Otherwise, if A is not of full rank, that is, the matrix is non-
square, or square and singular, then there are an infinite number of solutions which
are linear combinations of vectors in the right null space of A which is computed
by the SciPy function sp.linalg.null_space.
 751
F.2 Nonlinear Problems

F.1.3 Finding a Rotation Matrix

Consider two sets of points in Rn related by an unknown rotation R 2 SO.n/.

The points are arranged column-wise as P D fp0 ; : : : ; p m1 g 2 Rnm and Q D
fq 0 ; : : : ; q m1 g 2 Rnm such that

RP D Q :

To solve for R, we first compute the moment matrix

X
m1
MD q i p>
i
i D0

and then take the SVD such that M D U†V> . The least squares estimate of the
rotation matrix is
0 1
1 0 0  
R D U@0 1 0AV> ; s D det.U/ det V> (F.2)
0 0 s

and is guaranteed to be an SO.3/ matrix (Horn 1987; Umeyama 1991).

F.2 Nonlinear Problems

Many problems in robotics and computer vision involve sets of nonlinear equa-
tions. Solution of these problems requires linearizing the equations about an es-
timated solution, solving for an improved solution and iterating. Linearization is
discussed in 7 App. E.

F.2.1 Finding Roots

Consider a set of equations expressed in the form

f .x/ D 0 (F.3)

where f W Rn 7! Rm . This is a nonlinear version of (F.1), and we first linearize the

equations about our best estimate of the solution x 0

1 >
f .x 0 C / D f 0 C J.x 0 / C H.x 0 / C H.O.T. (F.4)
2

where f 0 D f .x 0 / 2 Rm1 is the function value and J D J.x 0 / 2 Rmn the Jaco-
bian, both evaluated at the linearization point, 2 Rn1 is an infinitesimal change
in x relative to x 0 and H.O.T. denotes higher-order terms. H.x 0 / is the n  n  m
Hessian tensor. 7 To evaluate > H.x 0 / we write it as
>
We take the first two terms of (F.4) to form a linear approximation N 2 / which uses a mode-2
.H.x 0 /
tensor-vector product.
f 0 . /  f 0 C J.x 0 / (F.5)

and solve an approximation of the original problem f 0 . / D 0

f 0 C J.x 0 / D0 ) D J1 .x 0 /f 0 :
752 F Solving Systems of Equations

If n ¤ m, then J is non-square and we can use the pseudoinverse np.linalg.

pinv(J), or a least squares solver scipy.linalg.spsolve(J, f0). The computed
step is based on an approximation to the original nonlinear function so x 0 C
will generally not be the solution but it will be closer. This leads to an iterative
solution – the Newton-Raphson method:

repeat
compute f 0 D f .x 0 /; J D J.x 0 /
J 1f 0
x0 x0 C
until kf 0 k

F.2.2 Nonlinear Minimization

A very common class of problems involves finding the minimum of a scalar func-
tion f .x/W Rn 7! R which can be expressed as
x  D arg min f .x/ :
x

The derivative of the linearized system of equations (F.5) is

df 0
D J.x 0 /
d
and if we consider the function to be a multi-dimensional surface, then J.x 0 / 2
Rn1 is a vector indicating the direction and magnitude of the slope at x D x 0 so
an update of
D ˇJ.x 0 /
will move the estimate down hill toward the minimum. This leads to an iterative
solution called gradient descent:

and the challenge is to choose the appropriate value of ˇ which controls the step
size. If too small, then many iterations will be required. If too large, the solution
may be unstable and not converge.
If we include the second-order term from (F.4), the approximation becomes
1
f 0 . /  f 0 C J.x 0 / C >
H.x 0 /
2
and includes the n  n Hessian matrix. To find its minima we take the derivative
and set it to zero
df 0
D 0 ) J.x 0 / C H.x 0 / D0
d
and the update is
D H1 .x 0 /J.x 0 / :
This leads to another iterative solution – Newton’s method. The challenge is deter-
mining the Hessian of the nonlinear system, either by numerical approximation or
symbolic manipulation.
 753
F.2 Nonlinear Problems

F.2.3 Nonlinear Least-Squares Minimization

Very commonly, the scalar function we wish to optimize is a quadratic cost function

F .x/ D jjf .x/jj2 D f .x/> f .x/

where f .x/W Rn 7! Rm is some vector-valued nonlinear function which we can

linearize as

f 0. /  f 0 C J

and the scalar cost is

F . /  .f 0 C J /> .f 0 C J /
 f 0> f 0 C f 0> J C > >
J f0 C > >
J J
 f 0> f 0 C 2f 0> J C > >
J J

where J> J 2 Rnn is the approximate Hessian from 7 App. E.2. 7 One of the underlined terms is the
To minimize the error of this linearized least squares system, we take the deriva- transpose of the other, but since both
tive with respect to and set it to zero result in a scalar, the transposition
doesn’t matter.
dF
D 0 ) 2f 0> J C > >
J JD0
d
which we can solve for the locally optimal update
 1
D  J> J J> f 0 (F.6)

which is the pseudoinverse or left generalized-inverse of J. Once again, we iterate

to find the solution – a Gauss-Newton iteration.

Numerical Issues
When solving (F.6), we may find that the Hessian J> J is poorly conditioned or
singular and we can add a damping term
 1
D  J> J C 1nn J> f 0

which makes the system more positive-definite. Since J> J C 1nn is effectively in
the denominator, increasing will decrease k k and slow convergence.
How do we choose ? We can experiment with different values, but a better
way is the Levenberg-Marquardt algorithm (. Fig. F.1) which adjusts to ensure
convergence. If the error increases compared to the last step, then the step is re-
peated with increased to reduce the step size. If the error decreases, then is
reduced to increase the convergence rate. The updates vary continuously between
Gauss-Newton (low ) and gradient descent (high ).
For problems where n is large, inverting the n  n approximate Hessian is ex-
pensive. Typically, m < n which means the Jacobian is not square and (F.6) can be
rewritten as
 1
D J> JJ> f 0

which is the right pseudoinverse and involves inverting a smaller matrix. We can
reintroduce a damping term
 1
D J> JJ> C 1mm f 0
754 F Solving Systems of Equations

1 initialize
2 repeat
3 compute f 0 D f .x 0 /, J D J.x 0 /, H D J> J
4 .H C 1n n / 1 J> f 0
5 if f .x 0 C / < f .x 0 / then
6 – error decreased: reduce damping
7 x0 x0 C
8
9 else
10 – error increased: discard and raise damping
11
12 end
13 until k k

. Fig. F.1 Levenberg-Marquadt algorithm, c is typically chosen in the range 2 to 10

and if is large this becomes simply

 ˇJ> f 0

but exhibits very slow convergence.

If f k . / has additive noise that is zero mean, normally distributed and time
invariant, we have a maximum likelihood estimator of x. Outlier data has a signif-
icant impact on the result since errors are squared. Robust estimators minimize the
effect of outlier data and in an M-estimator

F .x/ D .f k .x//

the squared norm is replaced by a loss function . / which models the likelihood
of its argument. Unlike the squared norm, these functions flatten off for large val-
ues, and some common examples include the Huber loss function and the Tukey
biweight function.

F.2.4 Sparse Nonlinear Least Squares

For a large class of problems, the overall cost is the sum of quadratic costs
X X
F .x/ D kf k .x/k2 D f k .x/> f k .x/ : (F.7)
k k

Consider the problem of fitting a model z D .wI x/ where W Rp 7! Rm with

parameters x 2 Rn to a set of data points .wk ; zk /. The error vector associated with
the k th data point is

f k .x/ D zk  .wk I x/ 2 Rm

and minimizing (F.7) gives the optimal model parameters x.

Another example is pose-graph optimization as used for pose-graph SLAM
and bundle adjustment. Edge k in the graph connects vertices i and j and has an
associated cost f k . /W Rn 7! Rm

f k .x/ D eO k .x/  e #k (F.8)

 755
F.2 Nonlinear Problems

where e #k is the observed value of the edge parameter and eO k .x/ is the estimate
based on the state x of the pose graph. This is linearized

f k0 . /  f 0;k C Jk

and the squared error for the edge is

Fk .x/ D f k> .x/k f k .x/

where k 2 Rmm is a positive-definite constant matrix 7 which we combine as This can be used to specify the
 >   significance of the edge det.k / with
Fk . /  f 0;k C Jk k f 0;k C Jk respect to other edges, as well as the
> > > > > > relative significance of the elements of
 f 0;k k f 0;k C f 0;k k Jk C Jk k f 0;k C Jk k Jk
f k . /.
 ck C 2b>
k C >
Hk
>
where ck D f 0;k k f 0;k , b> > >
k D f 0;k k Jk and Hk D Jk k Jk . The total cost is the
sum of all edge costs
X
F. / D Fk . /
k
X 
 ck C 2b>
k C >
Hk
k
! !
X X X
 ck C 2 b>
k C >
Hk
k k k

 c C 2b> C >
H

where
X X
b> D >
f 0;k k Jk ; HD J>
k  k Jk
k k

are summations over the edges of the graph. Once they are computed, we proceed
as previously, taking the derivative with respect to and setting it to zero, solving
for the update and iterating using . Fig. F.2.

State Vector
The state vector is a concatenation of all poses and coordinates in the optimization
problem. For pose-graph SLAM, it takes the form

x D f 0 ;  1 ; : : : ;  N 1 g 2 RNx :

Poses must be represented in a vector form and preferably one that is compact and
singularity free. For SE.2/, this is quite straightforward and we use   .x; y; / 2
R3 , and N D 3. For SE.3/, we will use   .t; r/ 2 R6 which comprises translation
t 2 R3 and rotation r 2 R3 , and N D 6. Rotation r can be triple angles (Euler or
roll-pitch-yaw), axis-angle (Euler vector), exponential coordinates or the vector
part of a unit quaternion as discussed in 7 Sect. 2.3.1.7. The state vector x has
length Nx D N N , and comprises a sequence of subvectors one per pose – the i th
subvector is x i 2 RN .
For pose-graph SLAM with landmarks, or bundle adjustment, the state vector
comprises poses and coordinate vectors

x D f 0 ;  1 ; : : : ;  N 1 jP 0 ; P 1 ; : : : ; P M 1 g 2 RNx

and we denote the i th and j th subvectors of x as x i 2 RN and xj 2 RNP that

correspond to  i and Pj respectively, where NP D 2 for SE.2/ and NP D 3 for
SE.3/. The state vector x length is Nx D N N C MNP .
756 F Solving Systems of Equations

a b

. Fig. F.2 Inherent structure of the error vector, Jacobian and Hessian matrices for graph-based least-squares problems. a Pose-graph SLAM with
N vertices representing robot pose as RN ; b bundle adjustment with N vertices representing camera pose as RN and M vertices representing
landmark position as RNP . The indices i and j denote the i th and j th block – not the i th and j th row or column. Zero values are indicated by white

Inherent Structure
A key observation is that the error vector f k .x/ for edge k depends only on the
associated vertices i and j , and this means that the Jacobian

@f k .x/
Jk D 2 RmNx
@x
is mostly zeros
  @f k .x/ @f k .x/
Jk D 0 Ai Bj 0 ; Ai D ; Bj D
@x i @xj

where Ai 2 RmN and Bj 2 RmN or Bj 2 RmNP according to the state vector

structure.
This sparse block structure means that the vector bk and the Hessian J> k  k Jk
also have a sparse block structure as shown in . Fig. F.2. The Hessian has just four
small nonzero blocks, so rather than compute the product J> k k Jk , which involves
many multiplications by zero, we can just compute the four nonzero blocks and
add them into the Hessian for the least-squares system. All blocks in a row have
the same height, and in a column have the same width. For pose-graph SLAM
with landmarks, or bundle adjustment, the blocks are of different sizes as shown in
. Fig. F.2b.
If the value of an edge represents pose, then (F.8) must be replaced with
f k .x/ D eO k .x/ e #k . We generalize this with the ˇ operator to indicate the use
of  or , subtraction or relative pose, as appropriate. Similarly, when updat-
ing the state vector at the end of an iteration, the poses must be compounded
x0 x 0 ˚ and positions incremented by x 0 x 0 C which we generalize
with the  operator. The pose-graph optimization is solved by the iteration in
. Fig. F.3.
 757
F.2 Nonlinear Problems

1 repeat
2 H 0; b 0
3 foreach k do
4 f 0;k .x 0 / D eO k .x/ ˇ e #k
5 .i; j / D vertices.k/
6 compute Ai .x i /, Bj .xj /
>
7 bi bi C f 0;k k Ai
>
8 bj bj C f 0;k k Bj
9 Hi;i Hi;i C A> i k Ai
>
10 Hi;j Hi;j C Ai k Bj
11 Hj;i Hj;i C Bj> k Ai
12 Hj;j Hj;j C Bj> k Bj
13 end
14 H 1
b
15 x0 x0
16 until k k

. Fig. F.3 Pose graph optimization. For Levenberg-Marquardt optimization, replace line 14 with
lines 4–12 from . Fig. F.1

Large Scale Problems

For pose-graph SLAM with thousands of poses or bundle adjustment with thou-
sands of cameras and millions of landmarks, the Hessian matrix will be massive
leading to computation and storage challenges. The overall Hessian is the sum-
mation of many edge Hessians structured as shown in . Fig. F.2, and the total
Hessian for two problems we have discussed are shown in . Fig. F.4. They have
clear structure which we can exploit.

0 0

50
1000

100

2000
150

3000 200

250
4000

300

5000
350

0 1000 2000 3000 4000 5000 0 50 100 150 200 250 300 350
a nz = 67795 b nz = 20646

. Fig. F.4 Hessian sparsity maps produced using the plt.spy function, the number of nonzero elements is shown beneath the plot. a Hessian for
the pose-graph SLAM problem of . Fig. 6.18, the diagonal elements represent pose constraints between successive vertices due to odometry, the
off-diagonal terms represent constraints due to revisiting locations (loop closures); b Hessian for a bundle adjustment problem with 10 cameras and
110 landmarks (examples/bademo)
758 F Solving Systems of Equations

Firstly, in both cases the Hessian is sparse – that is, it contains mostly zeros.
Instead of storing all those zeros (at 8 bytes each) we can employ sparse matrix
techniques that maintain a list of the nonzero elements. SciPy provides classes in
scipy.sparse to create sparse matrices which have similar behavior to nonsparse
NumPy arrays, as well as spsolve to solve Ax D b where the matrices and vectors
are sparse.
Secondly, for the bundle adjustment case in . Fig. F.2b, we see that the Hessian
has a block structure, so we partition the system as
! ! !
B E  b
D
E> C P bP

A block diagonal matrix is inverted by where B and C are block diagonal. 9 The subscripts  and P denote the blocks
simply inverting each of the nonzero of and b associated with camera poses and landmark positions respectively. We
blocks along its diagonal. solve first for the camera pose updates 

S  D b  EC1 bP

where S D B  EC1 E> is the Schur complement of C and is a symmetric positive-

definite matrix that is also block diagonal. Then we solve for the update to land-
mark positions
 
P D C1 bP  E>  :

More sophisticated techniques exploit the fine-scale block structure to further

reduce computational time, for example GTSAM (7 https://github.com/borglab/
gtsam) and SLAM++ (7 https://sourceforge.net/projects/slam-plus-plus).

Anchoring
Optimization provides a solution where the relative poses and positions give the
lowest overall cost, and the solution will have an arbitrary transformation with
respect to a global reference frame. To obtain absolute poses and positions, we
must anchor or fix some vertices – assign them values with respect to the global
frame and prevent the optimization from adjusting them. The appropriate way to
achieve this is to remove from H and b the rows and columns corresponding to the
anchored poses and positions. We then solve a lower dimensional problem for 0
which will be shorter than x. Careful bookkeeping is required to add the subvectors
of 0 into x for the update.
 759
G Gaussian Random Variables

G Gaussian Random Variables

The 1-dimensional Gaussian function

1  1 .x/2
g.x/ D p e 2 2 (G.1)
 22

is completely described by the position of its peak  and its width  . The total area
under the curve is unity and g.x/ > 0, 8x. The function can be plotted using the
RVC Toolbox function
>>> x = np.linspace(-6, 6, 500);
>>> plt.plot(x, gauss1d(0, 1, x), "r");
>>> plt.plot(x, gauss1d(0, 2**2, x), "b--");

and . Fig. G.1 shows two Gaussians with zero mean and  D 1 and  D 2. Note
that the second argument to gauss1d is  2 .
If the Gaussian is considered to be a probability density function (PDF), then
this is the well known normal distribution and the peak position  is the mean value
and the width  is the standard deviation. A random variable drawn from a normal
distribution is often written as X  N.;  2 ), and  2 is the variance. N.0; 1/ is re-
ferred to as the standard normal distribution – the NumPy function random.normal
draws random numbers from this distribution. To draw one hundred Gaussian ran-
dom numbers with mean mu and standard deviation sigma is
>>> g = np.random.normal(loc=mu, scale=sigma, size=(100,));

The probability that a random value falls within an interval x 2 Œx1 ; x2  is ob-
tained by integration

Zx2 Zx
P D g.x/ dx D ˆ.x2 /  ˆ.x1 /; where ˆ.x/ D g.x/ dx
x1 1

or evaluation of the cumulative normal distribution function ˆ.x/ returned by

scipy.stats.norm.cdf. The marked points in . Fig. G.1 at  ˙ 1 delimit the
1 confidence interval. The area under the curve over this interval is 0.68, so the
probability of a random value being drawn from this interval is 68%.

. Fig. G.1 Two Gaussian functions, both with mean  D 0, and with standard deviation  D 1, and
 D 2. The markers indicate the points x D  ˙ 1 . The area under both curves is unity
760 G Gaussian Random Variables

. Fig. G.2 The 2-dimensional Gaussian with covariance P D diag.12 ; 22 /. Contour lines of constant
probability density are shown beneath

The Gaussian can be extended to an arbitrary number of dimensions. The n-

dimensional Gaussian, or multivariate normal distribution, is
1 1
/> P1 .x /
g.x/ D p e 2 .x (G.2)
det.P/.2 /n

and compared to the scalar case of (G.1) x 2 Rn and 2 Rn have become vectors,
the squared term in the exponent has been replaced by a matrix quadratic form,
and  2 , the variance, has become a symmetric positive-definite matrix P 2 Rnn .
We can plot a 2-dimensional Gaussian
>>> x, y = np.meshgrid(np.linspace(-5, 5, 100),
... np.linspace(-5, 5, 100));
>>> P = np.diag([1, 2])**2;
>>> g = gauss2d([0, 0], P, x, y);
>>> ax = plotvol3();
>>> ax.plot_surface(x, y, g);
>>> ax.contour(x, y, g, zdir="z", offset=-0.05);

as a surface which is shown in . Fig. G.2. The contour lines are ellipses and in this
example, the radii in the y- and x-directions are in the ratio 2 W 1 as defined by the
ratio of the standard deviations. If the covariance matrix is diagonal, as in this case,
then the ellipses are aligned with the x- and y-axes as we saw in 7 App. C.1.4,
otherwise they are rotated.
If the 2-dimensional Gaussian is considered to be a PDF, then P is the covari-
ance matrix where the diagonal elements pi;i represent the variance of xi and the
off-diagonal elements pi;j are the correlationss between xi and xj . The inverse of
the covariance matrix is known as the information matrix. If the variables are inde-
pendent or uncorrelated, the matrix P would be diagonal. In this case, D .0; 0/
and P D diag.12 ; 22 / which corresponds to uncorrelated variables with standard
deviation of 1 and 2 respectively. If the PDF represented the position of a vehicle,
it is most likely to be at coordinates .0; 0/. The probability of being within any
region of the xy-plane is the volume under the surface of that region, and involves
a non-trivial integration to determine.
The connection between Gaussian probability density functions and ellipses
can be found in the quadratic exponent of (G.2) which is the equation of an ellipse
It is also the definition of Mahalanobis or ellipsoid. 9 All the points that satisfy
distance, the covariance weighted
distance between x and . .x  /> P1 .x  / D s

result in a constant probability density value, that is, a contour of the 2-dimensional
Gaussian. If p is the probability that the point x lies inside the ellipse, then s is
 761
G Gaussian Random Variables

given by the inverse-cumulative 2 distribution function 7 with n degrees of free- If we draw a vector of length n from
dom, 2 in this case. For example, the 50% confidence interval is the multivariate Gaussian, each
>>> from scipy.stats.distributions import chi2
element is normally distributed. The
>>> chi2.ppf(0.5, 2) sum of squares of independent
1.386 normally distributed values has a 2
(chi-squared) distribution with
where the first argument is the probability and the second is the number of degrees
n degrees of freedom.
of freedom.
To draw a 2-dimensional covariance ellipse, we use the general approach for
ellipses outlined in 7 App. C.1.4, but the right-hand side of the ellipse equation is s
not 1, and E D P1 . Using the Toolbox this is plot_ellipse(P, confidence=0.5,
inverted=True).
762 H Kalman Filter

H Kalman Filter

» All models are wrong. Some models are useful.

– George Box

Consider the system shown in . Fig. H.1. The physical robot is a “black box”
which has a true state or pose x that evolves over time according to the applied
inputs. We cannot directly measure the state, but sensors on the robot have outputs
which are a function of that true state. Our challenge is: given the system inputs
and sensor outputs, estimate the unknown true state x and how certain we are of
that estimate.
At face value, this might seem hard or even impossible, but we will assume
Often called the process or motion that the system has the model shown inside the black box, and we know how this
model. system behaves. Firstly, we know how the state evolves over time as a function of
the inputs – this is the state transition 9 model f . /, and we know the inputs u that
For example, wheel slippage on a are applied to the system. Our model is unlikely to be perfect 9 and it is common
mobile ground robot or wind gusts for to represent this uncertainty by an imaginary random number generator which is
a UAV. corrupting the system state – process noise. Secondly, we know how the sensor
output depends on the state – this is the sensor model h. / and its uncertainty is
also modeled by an imaginary random number generator – sensor noise.
The imaginary random number sources v and w are inside the black box so
the random numbers are also unknowable. However, we can describe the charac-
teristics of these random numbers – their distribution which tells us how likely it
is that we will draw a random number with a particular value. A lot of noise in
physical systems can be modeled well by the Gaussian (aka normal) distribution
N.;  2 / which is characterized by a mean  and a standard deviation  , and the
There are infinitely many possible Gaussian distribution has some nice mathematical properties that we will rely on. 9
distributions that these noise sources Typically, we assume that the noise has zero mean, and that the covariance of the
could have, which could be process and sensor noise are V and W respectively.
asymmetrical or have multiple peaks.
We should never assume that noise is
Gaussian – we should attempt to
determine the distribution by
understanding the physics of the
process and the sensor, or from careful
measurement and analysis.
z k+1 = h(x k+1 , w k )

N (0, W)
f (·) V̂
x̂ k+1
h(·) Ŵ P̂ k+1
x k+1 = f (x k , u k , v k )

N (0, V)

. Fig. H.1 The physical robot on the left is a “black box” whose true state cannot be directly measured.
However, it can be inferred from sensor output which is a function of the unknown true state, and the
assumed model shown inside the black box
 763
H.1 Linear Systems – Kalman Filter

In general terms, the problem we wish to solve is:

Given a model of the system f . /, h. /, estimates of V and W; the known inputs

applied to the system u; and some noisy sensor measurements z, find an estimate xO
of the system state and the uncertainty PO in that estimate.

In a robotic localization context, x is the unknown position or pose of the robot,

u is the commands sent to the motors, and z is the output of various sensors on the
robot. For a ground robot, x would be the pose in SE.2/, u would be the motor
commands, and z might be the measured odometry or range and bearing to land-
marks. For an aerial robot, x would be the pose in SE.3/, u are the known forces
applied to the airframe by the propellers, and z might be the measured accelerations
and angular velocities. 7 The state is a vector and there are
many approaches to mapping pose to a
vector, especially the rotational
H.1 Linear Systems – Kalman Filter component – Euler angles, quaternions,
and exponential coordinates are
Consider the transition model described as a discrete-time linear time-invariant commonly used, see 7 App. F.2.4.
system
x hkC1i D Fx hki C Guhki C vhki (H.1)
zhki D Hx hki C whki (H.2)
where k is the time step, x 2 Rn is the state vector, and u 2 Rm is a vector of
inputs to the system at time k, for example, a velocity command, or applied forces
and torques. The matrix F 2 Rnn describes the dynamics of the system, that is,
how the states evolve with time. The matrix G 2 Rnm describes how the inputs
are coupled to the system states. The vector z 2 Rp represents the outputs of the
system as measured by sensors. The matrix H 2 Rpn describes how the system
states are mapped to the system outputs which we can observe.
To account for errors in the motion model .F and G/ or unmodeled distur-
bances, we introduce a Gaussian random variable v 2 Rn termed the process noise.
vhki  N.0; V/, that is, it has zero mean and covariance V 2 Rnn . Covariance is
a matrix quantity which is the variance for a multi-dimensional distribution – it is
a symmetric positive-definite matrix. The sensor measurement model H is not per-
fect either, and this is modeled by sensor measurement noise, a Gaussian random
variable w 2 Rp , whki  N.0; W/ and covariance W 2 Rpp .
The Kalman filter is an optimal estimator for the case where the process and
measurement noise are both zero-mean Gaussian noise. The filter has two steps:
prediction and update. The prediction is based on the previous state and the inputs
that were applied

xO ChkC1i D FxO hki C Guhki (H.3)

C
PO hkC1i D FPO hkiF C V
> O (H.4)

where xO C is the predicted estimate of the state and PO 2 Rnn is the predicted es-
timate of the covariance, or uncertainty, in xO C . The notation C makes explicit that
the left-hand side is a prediction at time k C 1 based on information from time k.
O is a constant and our best estimate of the covariance of the process noise.
V
The underlined term in (H.4) projects the estimated covariance from the current
time step to the next. Consider a one dimensional example where F is a scalar
and the state estimate xO hki has a PDF which is Gaussian with a mean hki and a
764 H Kalman Filter

variance  2 hki. The prediction equation maps the state and its Gaussian distribution
to a new Gaussian distribution with a mean Fhki and a variance F 2  2 hki. The term
FhkiPhkiFhki> is the matrix form of this since

cov.Fx/ D F cov.x/F> (H.5)

which scales the covariance appropriately.

The prediction of PO in (H.4) involves the addition of two positive-definite ma-
trices so the uncertainty will increase – this is to be expected since we have used an
uncertain model to predict the future value of an already uncertain estimate. V O must
be a reasonable estimate of the covariance of the actual process noise. If we over-
estimate it, that is our estimate of process noise is larger than it really is, then we
will have a larger than necessary increase in uncertainty at this step, leading to a
pessimistic estimate of our certainty.
To counter this growth in uncertainty, we need to introduce new information
such as measurements made by the sensors since they depend on the state. The
difference between what the sensors measure and what the sensors are predicted to
measure is

 D z# hkC1i  HxO C hkC1i 2 Rp :

Some of this difference is due to noise in the sensor, the measurement noise, but
the remainder provides valuable information related to the error between the actual
and the predicted value of the state. Rather than considering this as error, we refer
to it more positively as innovation – new information.
The second step of the Kalman filter, the update step, maps the innovation into
a correction for the predicted state, optimally tweaking the estimate based on what
the sensors observed

xO hkC1i D xO C hkC1i C K; (H.6)

C C
PO hkC1i D PO hkC1i  KHPO hkC1i : (H.7)

Uncertainty is now decreased or deflated, since new information, from the sensors,
is being incorporated. The matrix

 1
O
K D PChkC1i H> HPChkC1iH> C W 2 Rnp (H.8)

is known as the Kalman gain. The term in parentheses is the estimated covariance of
the innovation, and comprises the uncertainty in the state and the estimated measure-
ment noise covariance. If the innovation has high uncertainty in some dimensions,
then the Kalman gain will be correspondingly small, that is, if the new information is
uncertain, then only small changes are made to the state vector. The underlined term
O is a
projects the covariance of the state estimate into the space of sensor values. W
constant and our best estimate of the covariance of the sensor noise.
The covariance matrix must be symmetric, but after many updates the accumu-
lated numerical errors may result in the matrix no longer being symmetric. The
symmetric structure can be enforced by using the Joseph form of (H.7)
C
PO hkC1i D .1nn  KH/PO hkC1i.1nn  KH/> C KVK
O >

but this is computationally more costly.

 765
H.2 Nonlinear Systems – Extended Kalman Filter

The equations above constitute the classical Kalman filter which is widely used
in robotics, aerospace and econometric applications. The filter has a number of
important characteristics. Firstly, it is optimal, but only if the noise is truly Gaussian
with zero mean and time-invariant parameters. This is often a good assumption but
not always. Secondly, it is recursive, the output of one iteration is the input to the
next. Thirdly, it is asynchronous. At a particular iteration, if no sensor information
is available, we only perform the prediction step and not the update. In the case that
there are different sensors, each with their own H, and different sample rates, we
just apply the update with the appropriate z and H whenever sensor data becomes
available.
The filter must be initialized with some reasonable value of xO and P, O as well as
good choices of the estimated covariance matrices V O and W. O As the filter runs, the
estimated covariance kPk O decreases but never reaches zero – the minimum value
can be shown to be a function of V O and W.O The Kalman-Bucy filter is a continuous-
time version of this filter.
The covariance matrix PO is rich in information. The diagonal elements pOi;i are
the variance, or uncertainty, in the state xi . The off-diagonal elements pOi;j are the
correlations between states xi and xj and indicate that the errors in the states are not
independent. The correlations are critical in allowing any piece of new information
to flow through to adjust all the states that affect a particular process output.
If elements of the state vector represent 2-dimensional position, the correspond-
ing 2  2 submatrix of PO is the inverse of the equivalent uncertainty ellipse E1 ,
see 7 App. C.1.4.

H.2 Nonlinear Systems – Extended Kalman Filter

For the case where the system is not linear, it can be described generally by two
functions: the state transition (the motion model in robotics) and the sensor model

x hkC1i D f .x hki; uhki; vhki/ (H.9)

zhki D h.x hki; whki/ (H.10)

and as before we represent model uncertainty, external disturbances and sensor

noise by Gaussian random variables v and w.
We linearize the state transition function about the current state estimate xO hki as
shown in . Fig. H.2 resulting in

x 0 hkC1i  Fx x 0 hki C Fu uhki C Fv vhki (H.11)

z0 hki  Hx x 0 hki C Hw whki (H.12)

where Fx D @f =@x 2 Rnn , Fu D @f =@u 2 Rnm , Fv D @f =@v 2 Rnn , Hx D

@h=@x 2 Rpn and Hw D @h=@w 2 Rpp are Jacobians, see 7 App. E, of the
functions f . / and h. /. Equating coefficients between (H.1) and (H.11) gives F 
Fx , G  Fu and vhki  Fv vhki; and between (H.2) and (H.12) gives H  Hx and
whki  Hw whki.
Taking the prediction equation (H.9) with vhki D 0, and the covariance equation
(H.4) with the linearized terms substituted, we can write the prediction step as

xO ChkC1i D f .xO hki; uhki/

C
PO hkC1i D Fx PO hkiF> O >
x C Fv VFv
766 H Kalman Filter

. Fig. H.2 One dimensional example illustrating how the nonlinear state transition function f : xk 7!
xkC1 shown in black is linearized about the point .xO hki; xO hkC1i/ shown in red

and the update step as

xO hkC1i D xO ChkC1i C K
C C
PO hkC1i D PO hkC1i  KHx PO hkC1i

where the Kalman gain is now

 1
K D PChkC1iH> C > O >
x Hx P hkC1iHx C Hw WHw : (H.13)

These equations are only valid at the linearization point xO hki – the Jacobians Fx , Fv ,
Properly, these matrices should be Hx , Hw must be computed at every iteration. 9 The full procedure is summarized
denoted as depending on the time step, in . Fig. H.3.
i.e. Fx hki, but this has been dropped in A fundamental problem with the extended Kalman filter is that PDFs of the
the interest of readability. random variables are no longer Gaussian after being operated on by the nonlinear
functions f . / and h. /. We can illustrate this by considering a nonlinear scalar
function y D .x C 2/2 =4. We will draw a million Gaussian random numbers from
the normal distribution N.5; 4/ which has a mean of 5 and a standard deviation of 2

>>> x = np.random.normal(loc=5, scale=2, size=(1_000_000,));

then apply the function

>>> y = (x + 2)**2 / 4;

and plot the probability density function of y

>>> plt.hist(y, bins=200, density=True, histtype="step");

which is shown in . Fig. H.4. We see that the PDF of y is substantially changed
and no longer Gaussian. It has lost its symmetry, so the mean value is greater
than the mode. The Jacobians that appear in the EKF equations appropriately scale
the covariance but the resulting non-Gaussian distribution breaks the assumptions
which guarantee that the Kalman filter is an optimal estimator. Alternative ap-
proaches to dealing with system nonlinearity include the iterated EKF described
 767
H.2 Nonlinear Systems – Extended Kalman Filter

Input: xO hki 2 Rn , PO hki 2 Rn n , uhki 2 Rm , zhkC1i 2 Rp ; V

O 2 Rn n ,
O
W2R p p

Output: xO hkC1i 2 Rn , PO hkC1i 2 Rn n

– linearize about x D xO hki

compute Jacobians: Fx 2 Rn n ; Fv 2 Rn n ; Hx 2 Rp n ; Hw 2 Rp p

– the prediction step

xO ChkC1i D f .xO hki; uhki/ // predict state at next time step

C
PO hkC1i D Fx PO hkiF> O >
x C Fv VFv // predict covariance at next time step

– the update step

D zhkC1i h.xO ChkC1i/ // innovation: measured - predicted sensor value

K D PChkC1iH> C > O > 1
x Hx P hkC1iHx C Hw WHw // Kalman gain
xO hkC1i D xO ChkC1i CK // update state estimate
C C
PO hkC1i D PO hkC1i KHx PO hkC1i // update covariance estimate

. Fig. H.3 Procedure EKF

. Fig. H.4 PDF of the state x which is Gaussian N.5; 4/ and the PDF of the nonlinear function y D
.x C 2/2 =4. The distribution of y is no longer Gaussian and the mean no longer corresponds to the peak

by Jazwinski (2007), the Unscented Kalman Filter (UKF) (Julier and Uhlmann
2004), or the sigma-point filter which uses discrete sample points (sigma points) to
approximate the PDF.
768 I Graphs

I Graphs

A graph is an abstract representation of a set of objects connected by links and

depicted visually as shown in . Fig. I.1. Mathematically, a graph is denoted by
G.V; E) where V are the vertices or nodes, and E are the links that connect pairs
of vertices and are called edges or arcs. Edges can be directed (shown as arrows) or
undirected (shown as line segments) as in this case. Edges can have an associated
weight or cost associated with moving from one vertex to another. A sequence of
edges from one vertex to another is a path, and a sequence that starts and ends at
the same vertex is a cycle. An edge from a vertex to itself is a loop. Graphs can be
used to represent transport, communications or social networks, and this branch of
mathematics is graph theory.
We will illustrate graphs using the Python package pgraph that supports em-
bedded graphs where the vertices are associated with a point in an n-dimensional
space. To create a new graph
>>> import pgraph
>>> g = pgraph.UGraph()

and, by default, the vertices of the graph exist in 2-dimensional space. We can add
five randomly-placed vertices to the graph
>>> np.random.seed(0) # ensure repeatable results
>>> for i in range(5):
... g.add_vertex(np.random.rand(2));

The method add_vertex returns a reference to the newly created vertex object, but
we can obtain the reference by indexing the graph object. For example vertex 1 is
>>> g[1]
UVertex[#1, coord=(0.6028, 0.5449)]

and is an instance of UVertex, a vertex in an undirected graph; it has the coordinate

.0:6028; 0:5449/; and has been assigned a default name "#1" but we could specify
Vertex names must be unique within a different name. 9 We can also reference a vertex by its name
the graph.
>>> g["#1"]
UVertex[#1, coord=(0.6028, 0.5449)]

We can create edges between pairs of vertex objects

>>> g.add_edge(g[0], g[1]);
>>> g.add_edge(g[0], g[2]);

. Fig. I.1 An example graph generated by the UGraph class

 769
I Graphs

>>> g.add_edge(g[0], g[3]);

>>> g.add_edge(g[1], g[2]);
>>> g.add_edge(g[1], g[3]);
>>> g.add_edge(g[3], g[4]);

and the add_edge method returns a reference to the edge object just created. By
default, the edge cost is the Euclidean distance between the vertices, but this can
be overridden by a third argument to add_edge.
A summary of the graph can be displayed
>>> print(g)
UGraph: 5 vertices, 6 edges, 1 component

This graph has one component, that is, all the vertices are connected into one net-
work. The graph can be plotted by
>>> g.plot()

as shown in . Fig. I.1. The vertices are shown as blue circles, and the edges are
shown as lines joining vertices. Many options exist to change default plotting be-
havior. Note that only graphs embedded in 2- and 3-dimensional space can be
plotted.
The neighbors of vertex 1 are
>>> g[1].adjacent()
[UVertex[#0, coord=(0.5488, 0.7152)],
UVertex[#2, coord=(0.4237, 0.6459)],
UVertex[#3, coord=(0.4376, 0.8918)]]

which is a list of neighboring vertex objects, those connected to it by an edge. Each

edge is an object and the edges connecting to vertex 1 are
>>> g[1].edges()
[Edge{[#0] -- [#1], cost=0.1786},
Edge{[#1] -- [#2], cost=0.2056},
Edge{[#1] -- [#3], cost=0.3842}]

and each edge has references to the vertices it connects

>>> g[1].edges()[0].endpoints
[UVertex[#0, coord=(0.5488, 0.7152)],
UVertex[#1, coord=(0.6028, 0.5449)]]

The cost or length of an edge is

>>> g[1].edges()[0].cost
0.1786

Arbitrary data can be attached to any vertex or edge by adding attributes to the
vertex or edge objects, or by subclassing the UVertex and Edge classes.
The vertex closest to the coordinate .0:5; 0:5/ is
>>> g.closest((0.5, 0.5))
(UVertex[#1, coord=(0.6028, 0.5449)], 0.1121)

The minimum-distance path between any two vertices in the graph can be com-
puted using well-known algorithms such as A (Hart et al. 1968)
>>> path, length, _ = g.path_Astar(g[2], g[4])
>>> path
[UVertex[#2, coord=(0.4237, 0.6459)],
UVertex[#0, coord=(0.5488, 0.7152)],
UVertex[#3, coord=(0.4376, 0.8918)],
UVertex[#4, coord=(0.9637, 0.3834)]]

which is a list of vertices representing a total path length of

>>> length
1.083

Methods exist to compute various other representations of the graph such as

adjacency, incidence, degree and Laplacian matrices.
770 J Peak Finding

J Peak Finding

A commonly encountered problem is finding, or refining an estimate of, the posi-

tion of the peak of some discrete 1- or 2-dimensional signal.

J.1 1D Signal

Consider the discrete 1-dimensional signal y.k/, k 2 N0

>>> y = mvtb_load_matfile("data/peakfit.mat")["y"];
>>> plt.plot(y, "-o");

shown in . Fig. J.1a.

Finding the position of the peak to the nearest integer k is straightforward using
the NumPy function argmax
>>> k = np.argmax(y)
7

which indicates the peak occurs at k D 7, and the peak value is

>>> y[k]
0.9905

In this case, there is more than one peak and we can use the RVC Toolbox function
findpeaks instead
>>> k, ypk = findpeaks(y)
>>> k
array([ 7, 0, 24, 15])
>>> ypk
array([ 0.9905, 0.873, 0.6718, -0.5799])

which has returned four maxima ordered by descending value. A common test of
the quality of a peak is its magnitude, and the ratio of the height of the second peak
to the first peak
>>> ypk[1] / ypk[0]
0.8813

which is called the ambiguity ratio and is ideally small.

a b

. Fig. J.1 Peak fitting. a A signal with several local maxima; b close-up view of the first maximum with the fitted parabola
 771
J.1 1D Signal

The signal y.k/ in . Fig. J.1a is a sampled representation of a continuous un-

derlying signal y.x/ and the real peak might actually lie between the samples. If
we look at a zoomed version of the signal, . Fig. J.1b, we can see that although
the maximum is at k D 7, the value at k D 8 is only slightly lower, so the peak lies
somewhere between these two points. A common approach is to fit a parabola
y D aıx2 C bıx C c; ıx 2 R (J.1)

to the points surrounding the peak: .1; yk1 /, .0; yk / and .1; ykC1 / where ıx D 0
when k D 7. Substituting these into (J.1), we can write three equations
yk1 D a  b C c
yk D c
ykC1 D a C b C c
or in compact matrix form as
0 1 0 10 1
yk1 1 1 1 a
@ yk A D @0 0 1A@b A
ykC1 1 1 1 c
and then solve for the parabolic coefficients
0 1 0 11 0 1 0 10 1
a 1 1 1 yk1 1 2 1 yk1
1
@b A D @0 0 1A @ yk A D @1 0 1A@ yk A : (J.2)
2
c 1 1 1 ykC1 0 2 0 ykC1
The maximum of the parabola occurs when its derivative is zero
2aıx C b D 0
and substituting the values of a and b from (J.2), we find the displacement of the
peak of the fitted parabola with respect to the discrete maximum
1 yk1  ykC1
ıx D ; ıx 2 .1; 1/
2 yk1  2yk C ykC1
so the refined, or interpolated, position of the maximum is at
xO D k C ıx 2 R
and the estimated value of the maximum is obtained by substituting ıx into (J.1).
The coefficient a will be negative for a maximum. If jaj is large, it indicates a
well-defined sharp peak, whereas a low value indicates a very broad peak for which
estimation of a refined peak may not be so accurate.
Continuing the earlier example, we can use the function findpeaks to estimate
the refined peak positions
>>> findpeaks(y, interp=True)
(array([ 7.439, 23.73, 15.24]), array([ 0.9969, 0.7037,
-0.578]))

where the argument interp uses a second-order polynomial by default, but this
can be overridden by setting interp to the desired polynomial order. The fitted
parabola for the first peak is shown in red in . Fig. J.1b.
If the signal has superimposed noise, then there are likely to be multiple peaks,
many of which are quite minor, and this can be overcome by specifying the scale
of the peak. For example, the peaks that are greater than all other values within
˙5 values in the horizontal direction are
>>> findpeaks(y, scale=5)
(array([ 7, 0, 24]), array([ 0.9905, 0.873, 0.6718]))
772 J Peak Finding

This technique is called nonlocal maxima suppression and requires that an appro-
priate scale is chosen. In this case, the result is unchanged since the signal is fairly
smooth.

J.2 2D Signal

An example 2-dimensional discrete signal can be loaded by

>>> img = mvtb_load_matfile("data/peakfit.mat")["image"]
array([[ -0.0696, 0.03483, 0.1394, 0.2436, 0.348],
[ 0.08, 0.3754, 0.3202, 0.44, 0.56],
[ 0.04003, 0.1717, 0.3662, 0.4117, 0.52],
[0.000214, 0.2062, 0.8766, 0.4462, 0.4802],
[-0.03997, 0.09166, 0.2862, 0.3317, 0.44],
[ -0.08, 0.04003, 0.1602, 0.28, 0.4]])

and the maximum value of 0.8766 is at [3, 2] using NumPy indexing conventions.
We can find this by
>>> k = np.argmax(img)
17

Starting with 0 at the top-left, across and the maximum is at element 17 in row-major order. 9 We can think of this as a
each row, then back to the left side of 1D index into a flattened version of the image
the next lower row, and so on. >>> img.ravel()[k]
0.8766

Alternatively, we convert to array subscripts

>>> np.unravel_index(k, img.shape)
(3, 2)

We can find this concisely using the RVC Toolbox function findpeaks2d
>>> xy = findpeaks2d(img)
array([[ 2, 3, 0.8766],
[ 1, 1, 0.3754]])

which has returned two local maxima, one per row, using the image coordinate
indexing convention. The third column is the peak value. This function will return
all nonlocal maxima where the size of the local region is given by the scale option
that is the half width of the 2D window in which the maximum is found.
We can refine the estimate of the 2D peak by following a similar procedure to
the 1D case. We fit a paraboloid

z D ax 2 C by 2 C cx C dy C e (J.3)

which has five coefficients that can be calculated from the center value (the dis-
crete maximum) and its four neighbors (north, south, east and west) using a similar
A paraboloid normally has an xy term procedure to above. 9 The displacement of the estimated peak with respect to the
giving a total of 6 parameters. This central point is
would require using one additional
point in order to solve the equation, 1 ze  zw
ıx D ; ıx 2 .1; 1/
which breaks the simplicity and 2 2zc  ze  zw
symmetry of this scheme. All 1 zn  za
ıy D ; ıy 2 .1; 1/
neighbors could be used, requiring a 2 2zc  zn  zs
least-squares solution.
The coefficients a and b represent the sharpness of the peak in the x- and y-
directions, and the quality of the peak can be considered as being min.jaj; jbj/.
 773
J.2 2D Signal

For this example, the refined peaks are given by

>>> xy = findpeaks2d(img, interp=True)
array([[ 2.109, 2.964, 0.8839, 0.5505],
[ 1.343, 1.126, 0.4003, 0.1753]])

which has one row per peak, comprising the coordinates, the interpolated height of
the peak, and the sharpness of the peak. The highest peak in this case is at image
coordinate .2:109; 2:964/ with a height of 0.8839. When this process is applied to
image data, it is referred to as subpixel interpolation.
774 References

References

Achtelik MW (2014) Advanced closed loop visual navigation for micro aerial vehicles. Ph.D. thesis,
ETH Zurich
Adorno B, Marinho M (2021) DQ Robotics: A Library for Robot Modeling and Control. IEEE Robotics
Automation Magazine, vol. 28, 102–116
Agarwal S, Furukawa Y, Snavely N, Simon I, Curless B, Seitz SM, Szeliski R (2011) Building Rome
in a day. Commun ACM 54(10):105–112
Agarwal P, Burgard W, Stachniss C (2014) Survey of geodetic mapping methods: Geodetic approaches
to mapping and the relationship to graph-based SLAM. IEEE Robot Autom Mag 21(3):63–80
Agrawal M, Konolige K, Blas M (2008) CenSurE: Center surround extremas for realtime feature de-
tection and matching. In: Forsyth D, Torr P, Zisserman A (eds) Lecture notes in computer science.
Computer Vision – ECCV 2008, vol 5305. Springer-Verlag, Berlin Heidelberg, pp 102–115
Albertos P, Mareels I (2010) Feedback and control for everyone. Springer-Verlag, Berlin Heidelberg
Altmann SL (1989) Hamilton, Rodrigues, and the quaternion scandal. Math Mag 62(5):291–308
Alton K, Mitchell IM (2006) Optimal path planning under defferent norms in continuous state spaces.
In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA).
pp 866–872
Andersen N, Ravn O, Sørensen A (1993) Real-time vision based control of servomechanical systems.
In: Chatila R, Hirzinger G (eds) Lecture notes in control and information sciences. Experimental
Robotics II, vol 190. Springer-Verlag, Berlin Heidelberg, pp 388–402
Andersson RL (1989) Dynamic sensing in a ping-pong playing robot. IEEE T Robotic Autom 5(6):728–
739
Antonelli G (2014) Underwater robots: Motion and force control of vehicle-manipulator systems, 3rd ed.
Springer Tracts in Advanced Robotics, vol 2. Springer-Verlag, Berlin Heidelberg
Arandjelović R, Zisserman A (2012) Three things everyone should know to improve object retrieval.
In: IEEE Conference on Computer Vision and Pattern Recognition (CVPR). pp 2911–2918
Arkin RC (1999) Behavior-based robotics. MIT Press, Cambridge, Massachusetts
Armstrong WW (1979) Recursive solution to the equations of motion of an N-link manipulator. In:
Proceedings of the 5th World Congress on Theory of Machines and Mechanisms, Montreal, Jul,
pp 1343–1346
Armstrong BS (1988) Dynamics for robot control: Friction modelling and ensuring excitation during
parameter identification. Stanford University
Armstrong B (1989) On finding exciting trajectories for identification experiments involving systems
with nonlinear dynamics. Int J Robot Res 8(6):28
Armstrong B, Khatib O, Burdick J (1986) The explicit dynamic model and inertial parameters of the
Puma 560 Arm. In: Proceedings of the IEEE International Conference on Robotics and Automation
(ICRA), vol 3. pp 510–518
Armstrong-Hélouvry B, Dupont P, De Wit CC (1994) A survey of models, analysis tools and compen-
sation methods for the control of machines with friction. Automatica 30(7):1083–1138
Arun KS, Huang TS, Blostein SD (1987) Least-squares fitting of 2 3-D point sets. IEEE T Pattern Anal
9(5):699–700
Asada H (1983) A geometrical representation of manipulator dynamics and its application to arm de-
sign. J Dyn Syst-T ASME 105:131
Astolfi A (1999) Exponential stabilization of a wheeled mobile robot via discontinuous control. J Dyn
Syst-T ASME 121(1):121–126
Azarbayejani A, Pentland AP (1995) Recursive estimation of motion, structure, and focal length. IEEE
T Pattern Anal 17(6):562–575
Bailey T, Durrant-Whyte H (2006) Simultaneous localization and mapping: Part II. IEEE Robot Autom
Mag 13(3):108–117
Bakthavatchalam M, Chaumette F, Tahri O (2015) An improved modelling scheme for photometric mo-
ments with inclusion of spatial weights for visual servoing with partial appearance/disappearance.
In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA).
pp 6037–6043
Baldridge AM, Hook SJ, Grove CI, Rivera G (2009) The ASTER spectral library version 2.0. Remote
Sens Environ 113(4):711–715
Ball RS (1876) The theory of screws: A study in the dynamics of a rigid body. Hodges, Foster & Co.,
Dublin
Ball RS (1908) A treatise on spherical astronomy. Cambridge University Press, New York
Ballard DH (1981) Generalizing the Hough transform to detect arbitrary shapes. Pattern Recogn
13(2):111–122
Banks J, Corke PI (2001) Quantitative evaluation of matching methods and validity measures for stereo
vision. Int J Robot Res 20(7):512–532
Barfoot T (2017) State Estimation for Robotics. Cambridge University Press
 775
References

Bar-Shalom Y, Fortmann T (1988) Tracking and data association. Mathematics in science and engineer-
ing, vol 182. Academic Press, London Oxford
Bar-Shalom Y, Li XR, Kirubarajan T (2001) Estimation with applications to tracking and navigation.
John Wiley & Sons, Inc., Chichester
Bateux Q, Marchand E, Leitner J, Chaumette F, Corke P (2018) Training Deep Neural Networks for
Visual Servoing. IEEE International Conference On Robotics And Automation (ICRA). pp 3307–
3314
Bauer J, Sünderhauf N, Protzel P (2007) Comparing several implementations of two recently published
feature detectors. In: IFAC Symposium on Intelligent Autonomous Vehicles (IAV). Toulouse
Bay H, Ess A, Tuytelaars T, Van Gool L (2008) Speeded-up robust features (SURF). Comput Vis Image
Und 110(3):346–359
Benosman R, Kang SB (2001) Panoramic vision: Sensors, theory, and applications. Springer-Verlag,
Berlin Heidelberg
Benson KB (ed) (1986) Television engineering handbook. McGraw-Hill, New York
Berns RS (2019) Billmeyer and Saltzman’s Principles of Color Technology, 4th ed. Wiley
Bertolazzi E, Frego M (2014) G1 fitting with clothoids. Mathematical Methods in the Applied Sciences
38(5):881–897
Bertozzi M, Broggi A, Cardarelli E, Fedriga R, Mazzei L, Porta P (2011) VIAC expedition: Toward
autonomous mobility. IEEE Robot Autom Mag 18(3):120–124
Besl PJ, McKay HD (1992) A method for registration of 3-D shapes. IEEE T Pattern Anal 14(2): 239–
256
Bhat DN, Nayar SK (2002) Ordinal measures for image correspondence. IEEE T Pattern Anal 20(4):
415–423
Biber P, Straßer W (2003) The normal distributions transform: A new approach to laser scan match-
ing. In: Proceedings of the IEEE/RSJ International Conference on intelligent robots and systems
(IROS), vol 3. pp 2743–2748
Bishop CM (2006) Pattern recognition and machine learning. Information science and statistics.
Springer-Verlag, New York
Blewitt M (2011) Celestial navigation for yachtsmen. Adlard Coles Nautical, London
Bolles RC, Baker HH, Marimont DH (1987) Epipolar-plane image analysis: An approach to determin-
ing structure from motion. Int J Comput Vision 1(1):7–55, Mar
Bolles RC, Baker HH, Hannah MJ (1993) The JISCT stereo evaluation. In: Image Understanding Work-
shop: proceedings of a workshop held in Washington, DC apr 18–21, 1993. Morgan Kaufmann, San
Francisco, pp 263
Bolton W (2015) Mechatronics: Electronic control systems in mechanical and electrical engineering,
6th ed. Pearson, Harlow
Borenstein J, Everett HR, Feng L (1996) Navigating mobile robots: Systems and techniques. AK Peters,
Ltd. Natick, MA, USA, Out of print and available at http://www-personal.umich.edu/~johannb/
Papers/pos96rep.pdf
Borgefors G (1986) Distance transformations in digital images. Comput Vision Graph 34(3):344–371
Bostrom N (2016) Superintelligence: Paths, dangers, strategies. Oxford University Press, Oxford, 432 p
Bouguet J-Y (2010) Camera calibration toolbox for MATLAB® . http://www.vision.caltech.edu/
bouguetj/calib_doc
Brady M, Hollerbach JM, Johnson TL, Lozano-Pérez T, Mason MT (eds) (1982) Robot motion: Plan-
ning and control. MIT Press, Cambridge, Massachusetts
Braitenberg V (1986) Vehicles: Experiments in synthetic psychology. MIT Press, Cambridge, Mas-
sachusetts
Bray H (2014) You are here: From the compass to GPS, the history and future of how we find ourselves.
Basic Books, New York
Brockett RW (1983) Asymptotic stability and feedback stabilization. In: Brockett RW, Millmann RS,
Sussmann HJ (eds) Progress in mathematics. Differential geometric control theory, vol 27. pp 181–
191
Broida TJ, Chandrashekhar S, Chellappa R (1990) Recursive 3-D motion estimation from a monocular
image sequence. IEEE T Aero Elec Sys 26(4):639–656
Brooks RA (1986) A robust layered control system for a mobile robot. IEEE T Robotic Autom 2(1):14–
23
Brooks RA (1989) A robot that walks: Emergent behaviors from a carefully evolved network. MIT AI
Lab, Memo 1091
Brown MZ, Burschka D, Hager GD (2003) Advances in computational stereo. IEEE T Pattern Anal
25(8):993–1008
Brynjolfsson E, McAfee A (2014) The second machine age: Work, progress, and prosperity in a time
of brilliant technologies. W.W. Norton & Co., New York
Buehler M, Iagnemma K, Singh S (eds) (2007) The 2005 DARPA grand challenge: The great robot
race. Springer Tracts in Advanced Robotics, vol 36. Springer-Verlag, Berlin Heidelberg
Buehler M, Iagnemma K, Singh S (eds) (2010) The DARPA urban challenge. Tracts in Advanced
Robotics, vol 56. Springer-Verlag, Berlin Heidelberg
776 References

Bukowski R, Haynes LS, Geng Z, Coleman N, Santucci A, Lam K, Paz A, May R, DeVito M (1991)
Robot hand-eye coordination rapid prototyping environment. In: Proc ISIR, pp 16.15–16.28
Buttazzo GC, Allotta B, Fanizza FP (1993) Mousebuster: A robot system for catching fast moving ob-
jects by vision. In: Proceedings of the IEEE International Conference on Robotics and Automation
(ICRA). Atlanta, pp 932–937
Calonder M, Lepetit V, Strecha C, Fua P (2010) BRIEF: Binary robust independent elementary features.
In: Daniilidis K, Maragos P, Paragios N (eds) Lecture notes in computer science. Computer Vision
– ECCV 2010, vol 6311. Springer-Verlag, Berlin Heidelberg, pp 778–792
Campos C, Elvira R, Rodríguez JJG, Montiel JMM, Tardós JD (2021) ORB-SLAM3: An Accurate
Open-Source Library for Visual, Visual–Inertial, and Multimap SLAM. IEEE Trans on Robotics,
37(6), pp 1874–1890
Canny JF (1983) Finding edges and lines in images. MIT, Artificial Intelligence Laboratory, AI-TR-720.
Cambridge, MA
Canny J (1987) A computational approach to edge detection. In: Fischler MA, Firschein O (eds) Read-
ings in computer vision: Issues, problems, principles, and paradigms. Morgan Kaufmann, San
Francisco, pp 184–203
Censi A (2008) An ICP variant using a point-to-line metric. In: Proceedings of the IEEE International
Conference on Robotics and Automation (ICRA). pp 19–25
Chahl JS, Srinivasan MV (1997) Reflective surfaces for panoramic imaging. Appl Optics 31(36):8275–
8285
Chaumette F (1990) La relation vision-commande: Théorie et application et des tâches robotiques.
Ph.D. thesis, Université de Rennes 1
Chaumette F (1998) Potential problems of stability and convergence in image-based and position-based
visual servoing. In: Kriegman DJ, Hager GD, Morse AS (eds) Lecture notes in control and informa-
tion sciences. The confluence of vision and control, vol 237. Springer-Verlag, Berlin Heidelberg,
pp 66–78
Chaumette F (2004) Image moments: A general and useful set of features for visual servoing. IEEE T
Robotic Autom 20(4):713–723
Chaumette F, Hutchinson S (2006) Visual servo control 1: Basic approaches. IEEE Robot Autom Mag
13(4):82–90
Chaumette F, Hutchinson S (2007) Visual servo control 2: Advanced approaches. IEEE Robot Autom
Mag 14(1):109–118
Chaumette F, Rives P, Espiau B (1991) Positioning of a robot with respect to an object, tracking it and
estimating its velocity by visual servoing. In: Proceedings of the IEEE International Conference on
Robotics and Automation (ICRA). Seoul, pp 2248–2253
Chesi G, Hashimoto K (eds) (2010) Visual servoing via advanced numerical methods. Lecture notes in
computer science, vol 401. Springer-Verlag, Berlin Heidelberg
Chiuso A, Favaro P, Jin H, Soatto S (2002) Structure from motion causally integrated over time. IEEE
T Pattern Anal 24(4):523–535
Choset HM, Lynch KM, Hutchinson S, Kantor G, Burgard W, Kavraki LE, Thrun S (2005) Principles
of robot motion. MIT Press, Cambridge, Massachusetts
Chum O, Matas J (2005) Matching with PROSAC - progressive sample consensus. IEEE Computer
Society Conference On Computer Vision And Pattern Recognition (CVPR), pp 220–226
Colicchia G, Waltner C, Hopf M, Wiesner H (2009) The scallop’s eye – A concave mirror in the context
of biology. Physics Education 44(2):175–179
Collewet C, Marchand E, Chaumette F (2008) Visual servoing set free from image processing. In:
Proceedings of IEEE International Conference on Robotics and Automation (ICRA). pp 81–86
Commission Internationale de L’Éclairage (1987) Colorimetry, 2nd ed. Commission Internationale de
L’Eclairage, CIE No 15.2
Corke PI (1994) High-performance visual closed-loop robot control. University of Melbourne, Dept.
Mechanical and Manufacturing Engineering. https://hdl.handle.net/11343/38847
Corke PI (1996a) In situ measurement of robot motor electrical constants. Robotica 14(4):433–436
Corke PI (1996b) Visual control of robots: High-performance visual servoing. Mechatronics, vol 2.
Research Studies Press (John Wiley). Out of print and available at http://www.petercorke.com/
bluebook
Corke PI (2001) Mobile robot navigation as a planar visual servoing problem. In: Jarvis RA, Zelinsky A
(eds) Springer tracts in advanced robotics. Robotics Research: The 10th International Symposium,
vol 6. IFRR, Lorne, pp 361–372
Corke PI (2007) A simple and systematic approach to assigning Denavit-Hartenberg parameters. IEEE
T Robotic Autom 23(3):590–594
Corke PI (2010) Spherical image-based visual servo and structure estimation. In: Proceedings of the
IEEE International Conference on Robotics and Automation (ICRA). Anchorage, pp 5550–5555
Corke PI, Armstrong-Hélouvry BS (1994) A search for consensus among model parameters reported
for the PUMA 560 robot. In: Proceedings of the IEEE International Conference on Robotics and
Automation (ICRA). San Diego, pp 1608–1613
 777
References

Corke PI, Armstrong-Hélouvry BS (1995) A meta-study of PUMA 560 dynamics: A critical appraisal
of literature data. Robotica 13(3):253–258
Corke PI, Good MC (1992) Dynamic effects in high-performance visual servoing. In: Proceedings of
the IEEE International Conference on Robotics and Automation (ICRA). Nice, pp 1838–1843
Corke PI, Good MC (1996) Dynamic effects in visual closed-loop systems. IEEE T Robotic Autom
12(5):671–683
Corke PI, Haviland J (2021) Not your grandmother’s toolbox – the Robotics Toolbox reinvented for
Python. IEEE International Conference on Robotics and Automation (ICRA). pp 11357–11363
Corke PI, Hutchinson SA (2001) A new partitioned approach to image-based visual servo control. IEEE
T Robotic Autom 17(4):507–515
Corke PI, Dunn PA, Banks JE (1999) Frame-rate stereopsis using non-parametric transforms and pro-
grammable logic. In: Proceedings of the IEEE International Conference on Robotics and Automa-
tion (ICRA). Detroit, pp 1928–1933
Corke PI, Lobo J, Dias J (2007) An introduction to inertial and visual sensing. The International Journal
of Robotics Research, 26(6). pp 519–536
Corke PI, Strelow D, Singh S (2004) Omnidirectional visual odometry for a planetary rover. In: Proceed-
ings of the International Conference on Intelligent Robots and Systems (IROS). Sendai, pp 4007–
4012
Corke PI, Spindler F, Chaumette F (2009) Combining Cartesian and polar coordinates in IBVS. In:
Proceedings of the International Conference on Intelligent Robots and Systems (IROS). St. Louis,
pp 5962–5967
Corke PI, Paul R, Churchill W, Newman P (2013) Dealing with shadows: Capturing intrinsic scene
appearance for image-based outdoor localisation. In: Proceedings of the IEEE/RSJ International
Conference on Intelligent Robots and Systems (IROS). pp 2085–2092
Craig JJ (1986) Introduction to robotics: Mechanics and control, 1st ed. Addison-Wesley
Craig JJ (1987) Adaptive control of mechanical manipulators. Addison-Wesley Longman Publishing
Co., Inc. Boston
Craig JJ (2005) Introduction to robotics: Mechanics and control, 3rd ed. Pearson/Prentice Hall
Craig JJ, Hsu P, Sastry SS (1987) Adaptive control of mechanical manipulators. Int J Robot Res 6(2):16–
28
Crombez N, Caron G, Mouaddib EM (2015) Photometric Gaussian mixtures based visual servoing. In:
Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).
pp 5486–5491
Crone RA (1999) A history of color: The evolution of theories of light and color. Kluwer Academic,
Dordrecht
Cummins M, Newman P (2008) FAB-MAP: Probabilistic localization and mapping in the space of
appearance. Int J Robot Res 27(6):647
Cutting JE (1997) How the eye measures reality and virtual reality. Behav Res Meth Ins C 29(1):27–36
Daniilidis K, Klette R (eds) (2006) Imaging beyond the pinhole camera. Computational Imaging, vol 33.
Springer-Verlag, Berlin Heidelberg
Dansereau DG (2014) Plenoptic signal processing for robust vision in field robotics. Ph.D. thesis, The
University of Sydney
Davies ER (2017) Computer Vision: Principles, Algorithms, Applications, Learning 5th ed. Academic
Press
Davison AJ, Reid ID, Molton ND, Stasse O (2007) MonoSLAM: Real-time single camera SLAM. IEEE
T Pattern Anal 29(6):1052–1067
Deguchi K (1998) Optimal motion control for image-based visual servoing by decoupling translation
and rotation. In: Proceedings of the International Conference on Intelligent Robots and Systems
(IROS). Victoria, Canada, pp 705–711
Dellaert F, Kaess M (2006) Square root SAM: Simultaneous localization and mapping via square root
information smoothing. Int J Robot Res 25(12):1181–1203
DeMenthon D, Davis LS (1992) Exact and approximate solutions of the perspective-three-point prob-
lem. IEEE T Pattern Anal 14(11):1100–1105
Denavit J, Hartenberg RS (1955) A kinematic notation for lower-pair mechanisms based on matrices. J
Appl Mech-T ASME 22(1):215–221
Deo AS, Walker ID (1995) Overview of damped least-squares methods for inverse kinematics of robot
manipulators. J Intell Robot Syst 14(1):43–68
Deriche R, Giraudon G (1993) A computational approach for corner and vertex detection. Int J Comput
Vision 10(2):101–124
Diankov R (2010) Automated Construction of Robotic Manipulation Programs. PhD thesis, Carnegie
Mellon University, Robotics Institute, CMU-RI-TR-10-29
Dickmanns ED (2007) Dynamic vision for perception and control of motion. Springer-Verlag, London
Dickmanns ED, Graefe V (1988a) Applications of dynamic monocular machine vision. Mach Vision
Appl 1:241–261
Dickmanns ED, Graefe V (1988b) Dynamic monocular machine vision. Mach Vision Appl 1(4):223–
240
778 References

Dickmanns ED, Zapp A (1987) Autonomous high speed road vehicle guidance by computer vision.
In: Tenth Triennial World Congress of the International Federation of Automatic Control, vol 4.
Munich, pp 221–226
Dijkstra EW (1959) A note on two problems in connexion with graphs. Numer Math 1(1):269–271
Dougherty ER, Lotufo RA (2003) Hands-on morphological image processing. Society of Photo-Optical
Instrumentation Engineers (SPIE)
Dubins LE (1957). On Curves of Minimal Length with a Constraint on Average Curvature, and with
Prescribed Initial and Terminal Positions and Tangents. American Journal of Mathematics, 79(3),
497–516
Duda RO, Hart PE (1972) Use of the Hough transformation to detect lines and curves in pictures.
Commun ACM 15(1):11–15
Durrant-Whyte H, Bailey T (2006) Simultaneous localization and mapping: Part I. IEEE Robot Autom
Mag 13(2):99–110
Espiau B, Chaumette F, Rives P (1992) A new approach to visual servoing in robotics. IEEE T Robotic
Autom 8(3):313–326
Everett HR (1995) Sensors for mobile robots: Theory and application. AK Peters Ltd., Wellesley
Faugeras OD (1993) Three-dimensional computer vision: A geometric viewpoint. MIT Press, Cam-
bridge, Massachusetts
Faugeras OD, Lustman F (1988) Motion and structure from motion in a piecewise planar environment.
Int J Pattern Recogn 2(3):485–508
Faugeras O, Luong QT, Papadopoulou T (2001) The geometry of multiple images: The laws that gov-
ern the formation of images of a scene and some of their applications. MIT Press, Cambridge,
Massachusetts
Featherstone R (1987) Robot dynamics algorithms. Springer
Featherstone R (2010a) A Beginner’s Guide to 6-D Vectors (Part 1). IEEE Robotics Automation Mag-
azine, vol. 17, 83–94
Featherstone R (2010b) A Beginner’s Guide to 6-D Vectors (Part 2). IEEE Robotics Automation Mag-
azine, vol. 17, 88–99
Feddema JT (1989) Real time visual feedback control for hand-eye coordinated robotic systems. Purdue
University
Feddema JT, Mitchell OR (1989) Vision-guided servoing with feature-based trajectory generation. IEEE
T Robotic Autom 5(5):691–700
Feddema JT, Lee CSG, Mitchell OR (1991) Weighted selection of image features for resolved rate
visual feedback control. IEEE T Robotic Autom 7(1):31–47
Felzenszwalb PF, Huttenlocher DP (2004) Efficient graph-based image segmentation. Int J Comput
Vision 59(2):167–181
Ferguson D, Stentz A (2006) Using interpolation to improve path planning: The Field D algorithm. J
Field Robotics 23(2):79–101
Fischler MA, Bolles RC (1981) Random sample consensus: A paradigm for model fitting with applica-
tions to image analysis and automated cartography. Commun ACM 24(6):381–395
Flusser J (2000) On the independence of rotation moment invariants. Pattern Recogn 33(9):1405–1410
Fomena R, Chaumette F (2007) Visual servoing from spheres using a spherical projection model. In:
Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). Rome,
pp 2080–2085
Ford M (2015) Rise of the robots: Technology and the threat of a jobless future. Basic Books, New York
Förstner W (1994) A framework for low level feature extraction. In: Ecklundh J-O (ed) Lecture notes
in computer science. Computer Vision – ECCV 1994, vol 800. Springer-Verlag, Berlin Heidelberg,
pp 383–394
Förstner W, Gülch E (1987) A fast operator for detection and precise location of distinct points, corners
and centres of circular features. In: ISPRS Intercommission Workshop. Interlaken, pp 149–155
Forsyth DA, Ponce J (2012) Computer vision: A modern approach, 2nd ed. Pearson, London
Fraundorfer F, Scaramuzza D (2012) Visual odometry: Part II – Matching, robustness, optimization,
and applications. IEEE Robot Autom Mag 19(2):78–90
Freeman H (1974) Computer processing of line-drawing images. ACM Comput Surv 6(1):57–97
Friedman DP, Felleisen M, Bibby D (1987) The little LISPer. MIT Press, Cambridge, Massachusetts
Frisby JP, Stone JV (2010) Seeing: The Computational Approach to Biological Vision. MIT Press
Fu KS, Gonzalez RC, Lee CSG (1987) Robotics: Control, Sensing, Vision, and Intelligence. McGraw-
Hill
Funda J, Taylor RH, Paul RP (1990) On homogeneous transforms, quaternions, and computational
efficiency. IEEE T Robotic Autom 6(3):382–388
Gans NR, Hutchinson SA, Corke PI (2003) Performance tests for visual servo control systems, with
application to partitioned approaches to visual servo control. Int J Robot Res 22(10–11):955
Garg R, Kumar B, Carneiro G, Reid I (2016) Unsupervised CNN for Single View Depth Estimation:
Geometry to the Rescue. ECCV
Gautier M, Khalil W (1992) Exciting trajectories for the identification of base inertial parameters of
robots. Int J Robot Res 11(4):362
 779
References

Geiger A, Roser M, Urtasun R (2010) Efficient large-scale stereo matching. In: Kimmel R, Klette R,
Sugimoto A (eds) Computer vision – ACCV 2010: 10th Asian Conference on Computer Vision,
Queenstown, New Zealand, November 8–12, 2010, revised selected papers, part I. Springer-Verlag,
Berlin Heidelberg, pp 25–38
Geraerts R, Overmars MH (2004) A comparative study of probabilistic roadmap planners. In: Bois-
sonnat J-D, Burdick J, Goldberg K, Hutchinson S (eds) Springer tracts in advanced robotics.
Algorithmic Foundations of Robotics V, vol 7. Springer-Verlag, Berlin Heidelberg, pp 43–58
Gevers T, Gijsenij A, van de Weijer J, Geusebroek J-M (2012) Color in computer vision: Fundamentals
and applications. John Wiley & Sons, Inc., Chichester
Geyer C, Daniilidis K (2000) A unifying theory for central panoramic systems and practical impli-
cations. In: Vernon D (ed) Lecture notes in computer science. Computer vision – ECCV 2000,
vol 1843. Springer-Verlag, Berlin Heidelberg, pp 445–461
Glover A, Maddern W, Warren M, Reid S, Milford M, Wyeth G (2012) OpenFABMAP: An open source
toolbox for appearance-based loop closure detection. In: Proceedings of the IEEE International
Conference on Robotics and Automation (ICRA). pp 4730–4735
Gonzalez RC, Woods RE (2018) Digital image processing, 4th ed. Pearson
Grassia FS (1998) Practical parameterization of rotations using the exponential map. Journal of Graph-
ics Tools 3(3):29–48
Gregory RL (1997) Eye and brain: The psychology of seeing. Princeton University Press, Princeton,
New Jersey
Grey CGP (2014) Humans need not apply. YouTube video, www.youtube.com/watch?v=7Pq-S557XQU
Grisetti G, Kümmerle R, Stachniss C, Burgard W (2010) A Tutorial on Graph-Based SLAM. IEEE
Intelligent Transportation Systems Magazine 2(4). pp 31–43
Groves PD (2013) Principles of GNSS, inertial, and multisensor integrated navigation systems, 2nd ed.
Artech House, Norwood, USA
Hager GD, Toyama K (1998) X Vision: A portable substrate for real-time vision applications. Comput
Vis Image Und 69(1):23–37
Hamel T, Mahony R (2002) Visual servoing of an under-actuated dynamic rigid-body system: An image
based approach. IEEE T Robotic Autom 18(2):187–198
Hamel T, Mahony R, Lozano R, Ostrowski J (2002) Dynamic modelling and configuration stabilization
for an X4-flyer. IFAC World Congress 1(2), p 3
Hansen P, Corke PI, Boles W (2010) Wide-angle visual feature matching for outdoor localization. Int J
Robot Res 29(1–2):267–297
Harris CG, Stephens MJ (1988) A combined corner and edge detector. In: Proceedings of the Fourth
Alvey Vision Conference. Manchester, pp 147–151
Hart PE (2009) How the Hough transform was invented [DSP history]. IEEE Signal Proc Mag 26(6):18–
22
Hart PE, Nilsson NJ, Raphael B (1968) A Formal Basis for the Heuristic Determination of Minimum
Cost Paths. IEEE Trans Systems Science and Cybernetics, 4(2):100–107
Hartenberg RS, Denavit J (1964) Kinematic synthesis of linkages. McGraw-Hill, New York
Hartley R, Zisserman A (2003) Multiple view geometry in computer vision. Cambridge University
Press, New York
Hashimoto K (ed) (1993) Visual servoing. In: Robotics and automated systems, vol 7. World Scientific,
Singapore
Hashimoto K, Kimoto T, Ebine T, Kimura H (1991) Manipulator control with image-based visual servo.
In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). Seoul,
pp 2267–2272
Heikkila J, and Silven O (1997) A four-step camera calibration procedure with implicit image cor-
rection, Proc IEEE Computer Society Conference on Computer Vision and Pattern Recognition
(CVPR), pp 1106–1112
Herschel W (1800) Experiments on the refrangibility of the invisible rays of the sun. Phil Trans R Soc
Lond 90:284–292
Hill J, Park WT (1979) Real time control of a robot with a mobile camera. In: Proceedings of the
9th ISIR, SME. Washington, DC. Mar, pp 233–246
Hirata T (1996) A unified linear-time algorithm for computing distance maps. Inform Process Lett
58(3):129–133
Hirschmüller H (2008) Stereo processing by semiglobal matching and mutual information. IEEE Trans-
actions on Pattern Analysis and Machine Intelligence 30(2):328–341
Hirt C, Claessens S, Fecher T, Kuhn M, Pail R, Rexer M (2013) New ultrahigh-resolution picture of
Earth’s gravity field. Geophys Res Lett 40:4279–4283
Hoag D (1963) Consideration of Apollo IMU gimbal lock. MIT Instrumentation Laboratory, E–1344,
https://www.hq.nasa.gov/alsj/e-1344.htm
Holland O (2003) Exploration and high adventure: the legacy of Grey Walter. Philosophical Trans of the
Royal Society of London. Series A: Mathematical, Physical and Engineering Sciences, 361(1811)
Hollerbach JM (1980) A recursive Lagrangian formulation of manipulator dynamics and a comparative
study of dynamics formulation complexity. IEEE T Syst Man Cyb 10(11):730–736, Nov
780 References

Hollerbach JM (1982) Dynamics. In: Brady M, Hollerbach JM, Johnson TL, Lozano-Pérez T, Mason
MT (eds) Robot motion – Planning and control. MIT Press, Cambridge, Massachusetts, pp 51–71
Horaud R, Canio B, Leboullenx O (1989) An analytic solution for the perspective 4-point problem.
Comput Vision Graph 47(1):33–44
Horn BKP (1987) Closed-form solution of absolute orientation using unit quaternions. J Opt Soc Am
A 4(4):629–642
Horn BKP, Hilden HM, Negahdaripour S (1988) Closed-form solution of absolute orientation using
orthonormal matrices. J Opt Soc Am A 5(7):1127–1135
Hosoda K, Asada M (1994) Versatile visual servoing without knowledge of true Jacobian. In: Pro-
ceedings of the International Conference on Intelligent Robots and Systems (IROS). Munich,
pp 186–193
Howard TM, Green CJ, Kelly A, Ferguson D (2008) State space sampling of feasible motions for high-
performance mobile robot navigation in complex environments. J Field Robotics 25(6–7):325–345
Hu MK (1962) Visual pattern recognition by moment invariants. IRE T Inform Theor 8:179–187
Hua M-D, Ducard G, Hamel T, Mahony R, Rudin K (2014) Implementation of a nonlinear attitude
estimator for aerial robotic vehicles. IEEE T Contr Syst T 22(1):201–213
Huang TS, Netravali AN (1994) Motion and structure from feature correspondences: A review. P IEEE
82(2):252–268
Humenberger M, Zinner C, Kubinger W (2009) Performance evaluation of a census-based stereo match-
ing algorithm on embedded and multi-core hardware. In: Proceedings of the 19th International
Symposium on Image and Signal Processing and Analysis (ISPA). pp 388–393
Hunt RWG (1987) The reproduction of colour, 4th ed. Fountain Press, Tolworth
Hunter RS, Harold RW (1987) The measurement of appearance. John Wiley & Sons, Inc., Chichester
Hutchinson S, Hager G, Corke PI (1996) A tutorial on visual servo control. IEEE T Robotic Autom
12(5):651–670
Huynh DQ (2009) Metrics for 3D Rotations: Comparison and Analysis. J Math Imaging Vis 35, pp
155–164
Ings S (2008) The Eye: A Natural History. Bloomsbury Publishing
Iwatsuki M, Okiyama N (2002a) A new formulation of visual servoing based on cylindrical coordinate
system with shiftable origin. In: Proceedings of the International Conference on Intelligent Robots
and Systems (IROS). Lausanne, pp 354–359
Iwatsuki M, Okiyama N (2002b) Rotation-oriented visual servoing based on cylindrical coordinates. In:
Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). Washing-
ton, DC, May, pp 4198–4203
Izaguirre A, Paul RP (1985) Computation of the inertial and gravitational coefficients of the dynamics
equations for a robot manipulator with a load. In: Proceedings of the IEEE International Conference
on Robotics and Automation (ICRA). Mar, pp 1024–1032
Jägersand M, Fuentes O, Nelson R (1996) Experimental evaluation of uncalibrated visual servoing for
precision manipulation. In: Proceedings of the IEEE International Conference on Robotics and
Automation (ICRA). Albuquerque, NM, pp 2874–2880
Jarvis RA, Byrne JC (1988) An automated guided vehicle with map building and path finding ca-
pabilities. In: Robotics Research: The Fourth international symposium. MIT Press, Cambridge,
Massachusetts, pp 497–504
Jazwinski AH (2007) Stochastic processes and filtering theory. Dover Publications, Mineola
Jebara T, Azarbayejani A, Pentland A (1999) 3D structure from 2D motion. IEEE Signal Proc Mag
16(3):66–84
Julier SJ, Uhlmann JK (2004) Unscented filtering and nonlinear estimation. P IEEE 92(3):401–422
Kaehler A, Bradski G (2017) Learning OpenCV 3: Computer vision in C++ with the OpenCV library.
O’Reilly & Associates, Köln
Kaess M, Ranganathan A, Dellaert F (2007) iSAM: Fast incremental smoothing and mapping with
efficient data association. In: Proceedings of the IEEE International Conference on Robotics and
Automation (ICRA). pp 1670–1677
Kahn ME (1969) The near-minimum time control of open-loop articulated kinematic linkages. Stanford
University, AIM-106
Kálmán RE (1960) A new approach to linear filtering and prediction problems. J Basic Eng-T Asme
82(1):35–45
Kane TR, Levinson DA (1983) The use of Kane’s dynamical equations in robotics. Int J Robot Res
2(3):3–21
Karaman S, Walter MR, Perez A, Frazzoli E, Teller S (2011) Anytime motion planning using the RRT*.
In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). pp
1478–1483
Kavraki LE, Svestka P, Latombe JC, Overmars MH (1996) Probabilistic roadmaps for path planning in
high-dimensional configuration spaces. IEEE T Robotic Autom 12(4):566–580
Kelly R (1996) Robust asymptotically stable visual servoing of planar robots. IEEE T Robotic Autom
12(5):759–766
 781
References

Kelly A (2013) Mobile robotics: Mathematics, models, and methods. Cambridge University Press, New
York
Kelly R, Carelli R, Nasisi O, Kuchen B, Reyes F (2002a) Stable visual servoing of camera-in-hand
robotic systems. IEEE-ASME T Mech 5(1):39–48
Kelly R, Shirkey P, Spong MW (2002b) Fixed-camera visual servo control for planar robots. In: Pro-
ceedings of the IEEE International Conference on Robotics and Automation (ICRA). Washington,
DC, pp 2643–2649
Kenwright K (2012) Dual-Quaternions: From Classical Mechanics to Computer Graphics and Beyond.
https://xbdev.net/misc_demos/demos/dual_quaternions_beyond/paper.pdf
Khalil W, Creusot D (1997) SYMORO+: A system for the symbolic modelling of robots. Robotica
15(2):153–161
Khalil W, Dombre E (2002) Modeling, identification and control of robots. Kogan Page Science, Lon-
don
Khatib O (1987) A unified approach for motion and force control of robot manipulators: The operational
space formulation. IEEE T Robotic Autom 3(1):43–53
King-Hele D (2002) Erasmus Darwin’s improved design for steering carriages and cars. Notes and
Records of the Royal Society of London 56(1):41–62
Klafter RD, Chmielewski TA, Negin M (1989) Robotic engineering – An integrated approach. Prentice
Hall, Upper Saddle River, New Jersey
Klein CA, Huang CH (1983) Review of pseudoinverse control for use with kinematically redundant
manipulators. IEEE T Syst Man Cyb 13:245–250
Klein G, Murray D (2007) Parallel tracking and mapping for small AR workspaces. In: Sixth IEEE and
ACM International Symposium on Mixed and Augmented Reality (ISMAR 2007). pp 225–234
Klette R, Kruger N, Vaudrey T, Pauwels K, van Hulle M, Morales S, Kandil F, Haeusler R, Pugeault N,
Rabe C (2011) Performance of correspondence algorithms in vision-based driver assistance using
an online image sequence database. IEEE T Veh Technol 60(5):2012–2026
Klette R (2014) Concise Computer Vision: An Introduction into Theory and Algorithms. Springer
Koenderink JJ (1984) The structure of images. Biol Cybern 50(5):363–370
Koenderink JJ (2010) Color for the sciences. MIT Press, Cambridge, Massachusetts
Koenig S, Likhachev M (2005) Fast replanning for navigation in unknown terrain. IEEE T Robotic
Autom 21(3):354–363
Krajník T, Cristóforis P, Kusumam K, Neubert P, Duckett T (2017) Image features for visual teach-and-
repeat navigation in changing environments. Robotics And Autonomous Systems. 88 pp 127–141
Kriegman DJ, Hager GD, Morse AS (eds) (1998) The confluence of vision and control. Lecture notes
in control and information sciences, vol 237. Springer-Verlag, Berlin Heidelberg
Kuipers JB (1999) Quaternions and rotation sequences: A primer with applications to orbits, aerospace
and virtual reality. Princeton University Press, Princeton, New Jersey
Kümmerle R, Grisetti G, Strasdat H, Konolige K, Burgard W (2011) g2 o: A general framework for graph
optimization. In: Proceedings of the IEEE International Conference on Robotics and Automation
(ICRA). pp 3607–3613
Lam O, Dayoub F, Schulz R, Corke P (2015) Automated topometric graph generation from floor plan
analysis. In: Proceedings of the Australasian Conference on Robotics and Automation. Australasian
Robotics and Automation Association (ARAA)
Lamport L (1994) LATEX: A document preparation system. User’s guide and reference manual.
Addison-Wesley Publishing Company, Reading
Land EH, McCann J (1971) Lightness and retinex theory. J Opt Soc Am A 61(1):1–11
Land MF, Nilsson D-E (2002) Animal eyes. Oxford University Press, Oxford
LaValle SM (1998) Rapidly-exploring random trees: A new tool for path planning. Computer Science
Dept., Iowa State University, TR 98–11
LaValle SM (2006) Planning algorithms. Cambridge University Press, New York
LaValle SM (2011a) Motion planning: The essentials. IEEE Robot Autom Mag 18(1):79–89
LaValle SM (2011b) Motion planning: Wild frontiers. IEEE Robot Autom Mag 18(2):108–118
LaValle SM, Kuffner JJ (2001) Randomized kinodynamic planning. Int J Robot Res 20(5):378–400
Laussedat A (1899) La métrophotographie. Enseignement supérieur de la photographie. Gauthier-
Villars, 52 p
Leavers VF (1993) Which Hough transform? Comput Vis Image Und 58(2):250–264
Lee CSG, Lee BH, Nigham R (1983) Development of the generalized D’Alembert equations of motion
for mechanical manipulators. In: Proceedings of the 22nd CDC, San Antonio, Texas. pp 1205–1210
Lepetit V, Moreno-Noguer F, Fua P (2009) EPnP: An accurate O(n) solution to the PnP problem. Int J
Comput Vision 81(2):155–166
Li H, Hartley R (2006) Five-point motion estimation made easy. In: 18th International Conference on
Pattern Recognition ICPR 2006. Hong Kong, pp 630–633
Li Y, Jia W, Shen C, van den Hengel A (2014) Characterness: An indicator of text in the wild. IEEE T
Image Process 23(4):1666–1677
Li T, Bolic M, Djuric P (2015) Resampling methods for particle filtering: Classification, implementa-
tion, and strategies. IEEE Signal Proc Mag 32(3):70–86
782 References

Lin Z, Zeman V, Patel RV (1989) On-line robot trajectory planning for catching a moving object. In:
Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). pp 1726–
1731
Lindeberg T (1993) Scale-space theory in computer vision. Springer-Verlag, Berlin Heidelberg
Lipkin H (2005) A Note on Denavit-Hartenberg Notation in Robotics. Proceedings of the 29th ASME
Mechanisms and Robotics Conference, pp 921–926
Lloyd J, Hayward V (1991) Real-time trajectory generation using blend functions. In: Proceedings of
the IEEE International Conference on Robotics and Automation (ICRA). Seoul, pp 784–789
Longuet-Higgins H (1981) A computer algorithm for reconstruction of a scene from two projections.
Nature 293:133–135
Lovell J, Kluger J (1994) Apollo 13. Coronet Books
Lowe DG (1991) Fitting parametrized three-dimensional models to images. IEEE T Pattern Anal 13(5):
441–450
Lowe DG (2004) Distinctive image features from scale-invariant keypoints. Int J Comput Vision
60(2):91–110
Lowry S, Sunderhauf N, Newman P, Leonard J, Cox D, Corke P, Milford M (2015) Visual place recog-
nition: A survey. Robotics, IEEE Transactions on (99):1–19
Lu F, Milios E (1997) Globally consistent range scan alignment for environment mapping. Auton Robot
4:333–349
Lucas SM (2005) ICDAR 2005 text locating competition results. In: Proceedings of the Eighth Interna-
tional Conference on Document Analysis and Recognition, ICDAR05. pp 80–84
Lucas BD, Kanade T (1981) An iterative image registration technique with an application to stereo
vision. In: International joint conference on artificial intelligence (IJCAI), Vancouver, vol 2. https://
www.ijcai.org/Past/%20Proceedings/IJCAI-81-VOL-2/PDF/017.pdf, pp 674–679
Luh JYS, Walker MW, Paul RPC (1980) On-line computational scheme for mechanical manipulators.
J Dyn Syst-T ASME 102(2):69–76
Lumelsky V, Stepanov A (1986) Dynamic path planning for a mobile automaton with limited informa-
tion on the environment. IEEE T Automat Contr 31(11):1058–1063
Luo W, Schwing A, Urtasun R (2016) Efficient Deep Learning for Stereo Matching. IEEE Conference
On Computer Vision And Pattern Recognition (CVPR).
Luong QT (1992) Matrice fondamentale et autocalibration en vision par ordinateur. Ph.D. thesis, Uni-
versité de Paris-Sud, Orsay, France
Lynch KM, Park FC (2017) Modern robotics: Mechanics, planning, and control. Cambridge University
Press, New York
Ma Y, Kosecka J, Soatto S, Sastry S (2003) An invitation to 3D. Springer-Verlag, Berlin Heidelberg
Ma J, Zhao J, Tian J, Yuille A, Tu Z (2014) Robust Point Matching via Vector Field Consensus, IEEE
Trans on Image Processing, 23(4), pp 1706–1721
Maddern W, Pascoe G, Linegar C, Newman P (2016) 1 Year, 1000km: The Oxford RobotCar Dataset.
The International Journal of Robotics Research 36(1). pp 3–15
Magnusson M, Lilienthal A, Duckett T (2007) Scan registration for autonomous mining vehicles using
3D-NDT. J Field Robotics 24(10):803–827
Magnusson M, Nuchter A, Lorken C, Lilienthal AJ, Hertzberg J (2009) Evaluation of 3D registration
reliability and speed – A comparison of ICP and NDT. In: Proceedings of the IEEE International
Conference on Robotics and Automation (ICRA). pp 3907–3912
Mahony R, Corke P, Chaumette F (2002) Choice of image features for depth-axis control in image based
visual servo control. IEEE/RSJ International Conference On Intelligent Robots And Systems, pp
390–395 vol.1
Mahony R, Kumar V, Corke P (2012) Multirotor aerial vehicles: Modeling, estimation, and control of
quadrotor. IEEE Robot Autom Mag (19):20–32
Maimone M, Cheng Y, Matthies L (2007) Two years of visual odometry on the Mars exploration rovers.
J Field Robotics 24(3):169–186
Makhlin AG (1985) Stability and sensitivity of servo vision systems. In: Proc 5th International Con-
ference on Robot Vision and Sensory Controls – RoViSeC 5. IFS (Publications), Amsterdam,
pp 79–89
Malis E (2004) Improving vision-based control using efficient second-order minimization techniques.
In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA).
pp 1843–1848
Malis E, Vargas M (2007) Deeper understanding of the homography decomposition for vision-based
control. Research Report, RR-6303, Institut National de Recherche en Informatique et en Automa-
tique (INRIA), 90 p, https://hal.inria.fr/inria-00174036v3/document
Malis E, Chaumette F, Boudet S (1999) 2-1/2D visual servoing. IEEE T Robotic Autom 15(2):238–250
Marey M, Chaumette F (2008) Analysis of classical and new visual servoing control laws. In: Pro-
ceedings of the IEEE International Conference on Robotics and Automation (ICRA). Pasadena,
pp 3244–3249
Mariottini GL, Prattichizzo D (2005) EGT for multiple view geometry and visual servoing: Robotics
vision with pinhole and panoramic cameras. IEEE T Robotic Autom 12(4):26–39
 783
References

Mariottini GL, Oriolo G, Prattichizzo D (2007) Image-based visual servoing for nonholonomic mobile
robots using epipolar geometry. IEEE T Robotic Autom 23(1):87–100
Marr D (2010) Vision: A computational investigation into the human representation and processing of
visual information. MIT Press, Cambridge, Massachusetts
Martin D, Fowlkes C, Tal D, Malik J (2001) A database of human segmented natural images and its
application to evaluating segmentation algorithms and measuring ecological statistics. Proceedings
of the 8th International Conference on Computer Vision, vol 2. pp 416–423
Martins FN, Celeste WC, Carelli R, Sarcinelli-Filho M, Bastos-Filho TF (2008) An adaptive dynamic
controller for autonomous mobile robot trajectory tracking. Control Eng Pract 16(11):1354–1363
Masutani Y, Mikawa M, Maru N, Miyazaki F (1994) Visual servoing for non-holonomic mobile robots.
In: Proceedings of the International Conference on Intelligent Robots and Systems (IROS). Munich,
pp 1133–1140
Matarić MJ (2007) The robotics primer. MIT Press, Cambridge, Massachusetts
Matas J, Chum O, Urban M, Pajdla T (2004) Robust wide-baseline stereo from maximally stable ex-
tremal regions. Image Vision Comput 22(10):761–767
Matthews ND, An PE, Harris CJ (1995) Vehicle detection and recognition for autonomous intelligent
cruise control. Technical Report, University of Southampton
Matthies L (1992) Stereo vision for planetary rovers: Stochastic modeling to near real-time implemen-
tation. Int J Comput Vision 8(1):71–91
Mayeda H, Yoshida K, Osuka K (1990) Base parameters of manipulator dynamic models. IEEE T
Robotic Autom 6(3):312–321
McGee LA, Schmidt SF (1985) Discovery of the Kalman filter as a Practical Tool for Aerospace and
Industry. NASA-TM-86847
McGlone C (ed) (2013) Manual of photogrammetry, 6th ed. American Society of Photogrammetry
McLauchlan PF (1999) The variable state dimension filter applied to surface-based structure from mo-
tion. University of Surrey, VSSP-TR-4/99
Merlet JP (2006) Parallel robots. Kluwer Academic, Dordrecht
Mettler B (2003) Identification modeling and characteristics of miniature rotorcraft. Kluwer Academic,
Dordrecht
Mičušík B, Pajdla T (2003) Estimation of omnidirectional camera model from epipolar geometry. In:
IEEE Conference on Computer Vision and Pattern Recognition, vol 1. Madison, pp 485–490
Middleton RH, Goodwin GC (1988) Adaptive computed torque control for rigid link manipulations.
Syst Control Lett 10(1):9–16
Mikolajczyk K, Schmid C (2004) Scale and affine invariant interest point detectors. Int J Comput Vision
60(1):63–86
Mikolajczyk K, Schmid C (2005) A performance evaluation of local descriptors. IEEE T Pattern Anal
27(10):1615–1630
Mindell DA (2008) Digital Apollo. MIT Press, Cambridge, Massachusetts
Molton N, Brady M (2000) Practical structure and motion from stereo when motion is unconstrained.
Int J Comput Vision 39(1):5–23
Montemerlo M, Thrun S (2007) FastSLAM: A scalable method for the simultaneous localization and
mapping problem in robotics, vol 27. Springer-Verlag, Berlin Heidelberg
Montemerlo M, Thrun S, Koller D, Wegbreit B (2003) FastSLAM 2.0: An improved particle filtering al-
gorithm for simultaneous localization and mapping that provably converges. In: Proceedings of the
18th International Joint Conference on Artificial Intelligence. Morgan Kaufmann, San Francisco,
pp 1151–1156
Moravec H (1980) Obstacle avoidance and navigation in the real world by a seeing robot rover. Ph.D.
thesis, Stanford University
Morel G, Liebezeit T, Szewczyk J, Boudet S, Pot J (2000) Explicit incorporation of 2D constraints in
vision based control of robot manipulators. In: Corke PI, Trevelyan J (eds) Lecture notes in control
and information sciences. Experimental robotics VI, vol 250. Springer-Verlag, Berlin Heidelberg,
pp 99–108
Muja M, Lowe DG (2009) Fast approximate nearest neighbors with automatic algorithm configura-
tion. International Conference on Computer Vision Theory and Applications (VISAPP), Lisbon,
Portugal (Feb 2009), pp 331–340
Murray RM, Sastry SS, Zexiang L (1994) A mathematical introduction to robotic manipulation. CRC
Press, Inc., Boca Raton
NASA (1970) Apollo 13: Technical air-to-ground voice transcription. Test Division, Apollo Spacecraft
Program Office, https://www.hq.nasa.gov/alsj/a13/AS13_TEC.PDF
Nayar SK (1997) Catadioptric omnidirectional camera. In: Proceedings of the IEEE Conference on
Computer Vision and Pattern Recognition. Los Alamitos, CA, pp 482–488
Neira J, Tardós JD (2001) Data association in stochastic mapping using the joint compatibility test.
IEEE T Robotic Autom 17(6):890–897
Neira J, Davison A, Leonard J (2008) Guest editorial special issue on Visual SLAM. IEEE T Robotic
Autom 24(5):929–931
784 References

Newcombe RA, Lovegrove SJ, Davison AJ (2011) DTAM: Dense tracking and mapping in real-time.
In: Proceedings of the International Conference on Computer Vision, pp 2320–2327
Ng J, Bräunl T (2007) Performance comparison of bug navigation algorithms. J Intell Robot Syst
50(1):73–84
Niblack W (1985) An introduction to digital image processing. Strandberg Publishing Company
Birkeroed, Denmark
Nistér D (2003) An efficient solution to the five-point relative pose problem. In: IEEE Conference on
Computer Vision and Pattern Recognition, vol 2. Madison, pp 195–202
Nistér D, Naroditsky O, Bergen J (2006) Visual odometry for ground vehicle applications. J Field
Robotics 23(1):3–20
Nixon MS, Aguado AS (2019) Feature extraction and image processing for Computer Vision, 4th ed.
Academic Press, London Oxford
Noble JA (1988) Finding corners. Image Vision Comput 6(2):121–128
Okutomi M, Kanade T (1993) A multiple-baseline stereo. IEEE T Pattern Anal 15(4):353–363
Ollis M, Herman H, Singh S (1999) Analysis and design of panoramic stereo vision using equi-angular
pixel cameras. Robotics Institute, Carnegie Mellon University, CMU-RI-TR-99-04, Pittsburgh, PA
Olson E (2011) AprilTag: A robust and flexible visual fiducial system. In: Proceedings of the IEEE
International Conference on Robotics and Automation (ICRA). pp 3400–3407
Orin DE, McGhee RB, Vukobratovic M, Hartoch G (1979) Kinematics and kinetic analysis of open-
chain linkages utilizing Newton-Euler methods. Math Biosci 43(1/2):107–130
Ortega R, Spong MW (1989) Adaptive motion control of rigid robots: A tutorial. Automatica 25(6):877–
888
Otsu N (1975) A threshold selection method from gray-level histograms. Automatica 11:285–296
Owen M, Beard RW, McLain TW (2015) Implementing Dubins Airplane Paths on Fixed-Wing UAVs.
Handbook of Uncrewed Aerial Vehicles, pp 1677–1701.
Papanikolopoulos NP, Khosla PK (1993) Adaptive robot visual tracking: Theory and experiments. IEEE
T Automat Contr 38(3):429–445
Papanikolopoulos NP, Khosla PK, Kanade T (1993) Visual tracking of a moving target by a camera
mounted on a robot: A combination of vision and control. IEEE T Robotic Autom 9(1):14–35
Patel S, Sobh T (2015) Manipulator performance measures-a comprehensive literature survey. Journal
Of Intelligent and Robotic Systems, vol. 77, pp 547–570
Paul R (1972) Modelling, trajectory calculation and servoing of a computer controlled arm. Ph.D. thesis,
technical report AIM-177, Stanford University
Paul R (1979) Manipulator Cartesian path control. IEEE T Syst Man Cyb 9:702–711
Paul RP (1981) Robot manipulators: Mathematics, programming, and control. MIT Press, Cambridge,
Massachusetts
Paul RP, Shimano B (1978) Kinematic control equations for simple manipulators. In: IEEE Conference
on Decision and Control, vol 17. pp 1398–1406
Paul RP, Zhang H (1986) Computationally efficient kinematics for manipulators with spherical wrists
based on the homogeneous transformation representation. Int J Robot Res 5(2):32–44
Piepmeier JA, McMurray G, Lipkin H (1999) A dynamic quasi-Newton method for uncalibrated vi-
sual servoing. In: Proceedings of the IEEE International Conference on Robotics and Automation
(ICRA). Detroit, pp 1595–1600
Pivtoraiko M, Knepper RA, Kelly A (2009) Differentially constrained mobile robot motion planning in
state lattices. J Field Robotics 26(3):308–333
Pock T (2008) Fast total variation for computer vision. Ph.D. thesis, Graz University of Technology
Pollefeys M, Nistér D, Frahm JM, Akbarzadeh A, Mordohai P, Clipp B, Engels C, Gallup D, Kim SJ,
Merrell P, et al. (2008) Detailed real-time urban 3D reconstruction from video. Int J Comput Vision
78(2):143–167, Jul
Pomerleau D, Jochem T (1995) No hands across America Journal. https://www.cs.cmu.edu/~tjochem/
nhaa/Journal.html
Pomerleau D, Jochem T (1996) Rapidly adapting machine vision for automated vehicle steering. IEEE
Expert 11(1):19–27
Posner I, Corke P, Newman P (2010) Using text-spotting to query the world. In: IEEE/RSJ International
Conference on Intelligent Robots and Systems (IROS). IEEE, pp 3181–3186
Pounds P (2007) Design, construction and control of a large quadrotor micro air vehicle. Ph.D. thesis,
Australian National University
Pounds P, Mahony R, Gresham J, Corke PI, Roberts J (2004) Towards dynamically-favourable quad-
rotor aerial robots. In: Proceedings of the Australasian Conference on Robotics and Automation.
Canberra
Pounds P, Mahony R, Corke PI (2006) A practical quad-rotor robot. In: Proceedings of the Australasian
Conference on Robotics and Automation. Auckland
Poynton CA (2012) Digital video and HD algorithms and interfaces. Morgan Kaufmann, Burlington
Press WH, Teukolsky SA, Vetterling WT, Flannery BP (2007) Numerical recipes, 3rd ed. Cambridge
University Press, New York
 785
References

Prince SJ (2012) Computer vision: Models, learning, and inference. Cambridge University Press, New
York
Prouty RW (2002) Helicopter performance, stability, and control. Krieger, Malabar FL
Pujol J (2012) Hamilton, Rodrigues, Gauss, Quaternions, and Rotations: A Historical Reassessment.
Communications In Mathematical Analysis, vol. 13
Pynchon T (2006) Against the day. Jonathan Cape, London
Reeds J, Shepp L (1990) Optimal paths for a car that goes both forwards and backwards. Pacific Journal
Of Mathematics, vol. 145, pp 367–393
Rekleitis IM (2004) A particle filter tutorial for mobile robot localization. Technical report (TR-CIM-
04-02), Centre for Intelligent Machines, McGill University
Rives P, Chaumette F, Espiau B (1989) Positioning of a robot with respect to an object, tracking it and
estimating its velocity by visual servoing. In: Hayward V, Khatib O (eds) Lecture notes in control
and information sciences. Experimental robotics I, vol 139. Springer-Verlag, Berlin Heidelberg,
pp 412–428
Rizzi AA, Koditschek DE (1991) Preliminary experiments in spatial robot juggling. In: Chatila R,
Hirzinger G (eds) Lecture notes in control and information sciences. Experimental robotics II,
vol 190. Springer-Verlag, Berlin Heidelberg, pp 282–298
Roberts LG (1963) Machine perception of three-dimensional solids. MIT Lincoln Laboratory, TR 315,
https://dspace.mit.edu/handle/1721.1/11589
Romero-Ramirez FJ, Muñoz-Salinas R, Medina-Carnicer R (2018) Speeded up detection of squared
fiducial markers. Image and Vision Computing, vol 76, pp 38–47
Rosenfeld GH (1959) The problem of exterior orientation in photogrammetry. Photogramm Eng
25(4):536–553
Rosten E, Porter R, Drummond T (2010) FASTER and better: A machine learning approach to corner
detection. IEEE T Pattern Anal 32:105–119
Russell S, Norvig P (2020) Artificial intelligence: A modern approach, 4th ed. Pearson
Sakaguchi T, Fujita M, Watanabe H, Miyazaki F (1993) Motion planning and control for a robot per-
former. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA).
Atlanta, May, pp 925–931
Salvi J, Matabosch C, Fofi D, Forest J (2007) A review of recent range image registration methods with
accuracy evaluation. Image Vision Comput 25(5):578–596
Samson C, Espiau B, Le Borgne M (1990) Robot control: The task function approach. Oxford Univer-
sity Press, Oxford
Scaramuzza D, Martinelli A, Siegwart R (2006) A Toolbox for Easy Calibrating Omnidirectional Cam-
eras. Proc IEEE/RSJ Int Conf on Intelligent Robots and Systems, pp 5695–5701
Scaramuzza D, Fraundorfer F (2011) Visual odometry [tutorial]. IEEE Robot Autom Mag 18(4):80–92
Scharstein D, Pal C (2007) Learning conditional random fields for stereo. In: IEEE Computer Society
Conference on Computer Vision and Pattern Recognition (CVPR). Minneapolis, MN
Scharstein D, Szeliski R (2002) A taxonomy and evaluation of dense two-frame stereo correspondence
algorithms. Int J Comput Vision 47(1):7–42
Selig JM (2005) Geometric fundamentals of robotics. Springer-Verlag, Berlin Heidelberg
Sharf I, Nahon M, Harmat A, Khan W, Michini M, Speal N, Trentini M, Tsadok T, Wang T (2014)
Ground effect experiments and model validation with Draganflyer X8 rotorcraft. Int Conf Un-
crewed Aircraft Systems (ICUAS). pp 1158–1166
Sharp A (1896) Bicycles & tricycles: An elementary treatise on their design an construction; With
examples and tables. Longmans, Green and Co., London New York Bombay
Sheridan TB (2003) Telerobotics, automation, and human supervisory control. MIT Press, Cambridge,
Massachusetts, 415 p
Shi J, Tomasi C (1994) Good features to track. In: Proceedings of the Computer Vision and Pattern
Recognition. IEEE Computer Society, Seattle, pp 593–593
Shih FY (2009) Image processing and mathematical morphology: Fundamentals and applications, CRC
Press, Boca Raton
Shirai Y (1987) Three-dimensional computer vision. Springer-Verlag, New York
Shirai Y, Inoue H (1973) Guiding a robot by visual feedback in assembling tasks. Pattern Recogn
5(2):99–106
Shoemake K (1985) Animating rotation with quaternion curves. In: Proceedings of ACM SIGGRAPH,
San Francisco, pp 245–254
Siciliano B, Khatib O (eds) (2016) Springer handbook of robotics, 2nd ed. Springer-Verlag, New York
Siciliano B, Sciavicco L, Villani L, Oriolo G (2009) Robotics: Modelling, planning and control.
Springer-Verlag, Berlin Heidelberg
Siegwart R, Nourbakhsh IR, Scaramuzza D (2011) Introduction to autonomous mobile robots, 2nd ed.
MIT Press, Cambridge, Massachusetts
Silver WM (1982) On the equivalence of Lagrangian and Newton-Euler dynamics for manipulators. Int
J Robot Res 1(2):60–70
Sivic J, Zisserman A (2003) Video Google: A text retrieval approach to object matching in videos. In:
Proceedings of the Ninth IEEE International Conference on Computer Vision. pp 1470–1477
786 References

Skaar SB, Brockman WH, Hanson R (1987) Camera-space manipulation. Int J Robot Res 6(4):20–32
Skofteland G, Hirzinger G (1991) Computing position and orientation of a freeflying polyhedron
from 3D data. In: Proceedings of the IEEE International Conference on Robotics and Automation
(ICRA). Seoul, pp 150–155
Smith R (2007) An overview of the Tesseract OCR engine. In: 9th International Conference on Docu-
ment Analysis and Recognition (ICDAR). pp 629–633
Sobel D (1996) Longitude: The true story of a lone genius who solved the greatest scientific problem
of his time. Fourth Estate, London
Soille P (2003) Morphological image analysis: Principles and applications. Springer-Verlag, Berlin Hei-
delberg
Solà J (2017) Quaternion kinematics for the error-state Kalman filter. arXiv 1711.02508
Solà J, Deray J, Atchuthan D (2018) A micro Lie theory for state estimation in robotics. arXiv
1812.01537
Spong MW (1989) Adaptive control of flexible joint manipulators. Syst Control Lett 13(1):15–21
Spong MW, Hutchinson S, Vidyasagar M (2006) Robot modeling and control, 2nd ed. John Wiley &
Sons, Inc., Chichester
Srinivasan VV, Venkatesh S (1997) From living eyes to seeing machines. Oxford University Press,
Oxford
Stachniss C, Burgard W (2014) Particle filters for robot navigation. Foundations and Trends in Robotics
3(4):211–282
Steinvall A (2002) English colour terms in context. Ph.D. thesis, Umeå Universitet
Stentz A (1994) The D algorithm for real-time planning of optimal traverses. The Robotics Institute,
Carnegie-Mellon University, CMU-RI-TR-94-37
Stewart A (2014) Localisation using the appearance of prior structure. Ph.D. thesis, University of Ox-
ford
Stone JV (2012) Vision and brain: How we perceive the world. MIT Press, Cambridge, Massachusetts
Strasdat H (2012) Local accuracy and global consistency for efficient visual SLAM. Ph.D. thesis, Im-
perial College London
Strelow D, Singh S (2004) Motion estimation from image and inertial measurements. Int J Robot Res
23(12):1157–1195
Sünderhauf N (2012) Robust optimization for simultaneous localization and mapping. Ph.D. thesis,
Technische Universität Chemnitz
Sussman GJ, Wisdom J, Mayer ME (2001) Structure and interpretation of classical mechanics. MIT
Press, Cambridge, Massachusetts
Sutherland IE (1974) Three-dimensional data input by tablet. P IEEE 62(4):453–461
Svoboda T, Pajdla T (2002) Epipolar geometry for central catadioptric cameras. Int J Comput Vision
49(1):23–37
Szeliski R (2022) Computer vision: Algorithms and applications. Springer-Verlag, Berlin Heidelberg
Tahri O, Chaumette F (2005) Point-based and region-based image moments for visual servoing of planar
objects. IEEE T Robotic Autom 21(6):1116–1127
Tahri O, Mezouar Y, Chaumette F, Corke PI (2009) Generic decoupled image-based visual servoing for
cameras obeying the unified projection model. In: Proceedings of the IEEE International Confer-
ence on Robotics and Automation (ICRA). Kobe, pp 1116–1121
Taylor RA (1979) Planning and execution of straight line manipulator trajectories. IBM J Res Dev
23(4):424–436
ter Haar Romeny BM (1996) Introduction to scale-space theory: Multiscale geometric image analysis.
Utrecht University
Thrun S, Burgard W, Fox D (2005) Probabilistic robotics. MIT Press, Cambridge, Massachusetts
Tissainayagam P, Suter D (2004) Assessing the performance of corner detectors for point feature track-
ing applications. Image Vision Comput 22(8):663–679
Titterton DH, Weston JL (2005) Strapdown inertial navigation technology. IEE Radar, Sonar, Naviga-
tion and Avionics Series, vol 17, The Institution of Engineering and Technology (IET), 576 p
Tomasi C, Kanade T (1991) Detection and tracking of point features. Carnegie Mellon University,
CMU-CS-91-132
Triggs B, McLauchlan P, Hartley R, Fitzgibbon A (2000) Bundle adjustment – A modern synthesis.
Lecture notes in computer science. Vision algorithms: theory and practice, vol 1883. Springer-
Verlag, Berlin Heidelberg, pp 153–177
Tsai RY (1986) An Efficient and Accurate Camera Calibration Technique for 3D Machine Vision. Proc
IEEE Conference on Computer Vision and Pattern Recognition (CVPR), pp 364–374
Tsakiris D, Rives P, Samson C (1998) Extending visual servoing techniques to nonholonomic mobile
robots. In: Kriegman DJ, Hager GD, Morse AS (eds) Lecture notes in control and information sci-
ences. The confluence of vision and control, vol 237. Springer-Verlag, Berlin Heidelberg, pp 106–
117
Terzakis G, Lourakis MIA, Ait-Boudaoud D (2018) Modified Rodrigues Parameters: An Efficient Rep-
resentation of Orientation in 3D Vision and Graphics. J Math Imaging Vis. 60:422–442
 787
References

Uicker JJ (1965) On the dynamic analysis of spatial linkages using 4 by 4 matrices. Dept. Mechanical
Engineering and Astronautical Sciences, NorthWestern University
Umeyama, S (1991) Least-Squares Estimation of Transformation Parameters Between Two Point Pat-
terns. IEEE Trans Pattern Analysis and Machine Intellence 13(4). pp 376–380
Usher K (2005) Visual homing for a car-like vehicle. Ph.D. thesis, Queensland University of Technology
Usher K, Ridley P, Corke PI (2003) Visual servoing of a car-like vehicle – An application of omnidirec-
tional vision. In: Proceedings of the IEEE International Conference on Robotics and Automation
(ICRA). Taipei, Sep, pp 4288–4293
Valgren C, Lilienthal AJ (2010) SIFT, SURF & seasons: Appearance-based long-term localization in
outdoor environments. Robot Auton Syst 58(2):149–156
Vanderborght B, Sugar T, Lefeber D (2008) Adaptable compliance or variable stiffness for robotic
applications. IEEE Robot Autom Mag 15(3):8–9
Vince J (2011) Quaternions for Computer Graphics. Springer
Wade NJ (2007) Image, eye, and retina. J Opt Soc Am A 24(5):1229–1249
Walker MW, Orin DE (1982) Efficient dynamic computer simulation of robotic mechanisms. J Dyn
Syst-T ASME 104(3):205–211
Walter WG (1950) An imitation of life. Sci Am 182(5):42–45
Walter WG (1951) A machine that learns. Sci Am 185(2):60–63
Walter WG (1953) The living brain. Duckworth, London
Warren M (2015) Long-range stereo visual odometry for unmanned aerial vehicles. Ph.D. thesis,
Queensland University of Technology
Weiss LE (1984) Dynamic visual servo control of robots: An adaptive image-based approach. Ph.D.
thesis, technical report CMU-RI-TR-84-16, Carnegie-Mellon University
Weiss L, Sanderson AC, Neuman CP (1987) Dynamic sensor-based control of robots with visual feed-
back. IEEE T Robotic Autom 3(1):404–417
Westmore DB, Wilson WJ (1991) Direct dynamic control of a robot using an end-point mounted camera
and Kalman filter position estimation. In: Proceedings of the IEEE International Conference on
Robotics and Automation (ICRA). Seoul, Apr, pp 2376–2384
Whitney DE (1969) Resolved motion rate control of manipulators and human prostheses. IEEE T Man
Machine 10(2):47–53
Wiener N (1965) Cybernetics or control and communication in the animal and the machine. MIT Press,
Cambridge, Massachusetts
Wilburn B, Joshi N, Vaish V, Talvala E-V, Antunez E, Barth A, Adams A, Horowitz M, Levoy M (2005)
High performance imaging using large camera arrays. ACM Transactions on Graphics (TOG) –
Proceedings of ACM SIGGRAPH 2005 24(3):765–776
Wolf PR, DeWitt BA (2014) Elements of photogrammetry, 4th ed. McGraw-Hill, New York
Woodfill J, Von Herzen B (1997) Real-time stereo vision on the PARTS reconfigurable computer. In:
Proceedings of the IEEE Symposium on FPGAs for Custom Computing Machines, Grenoble.
pp 201–210
Xu G, Zhang Z (1996) Epipolar geometry in stereo, motion, and object recognition: A unified approach.
Springer-Verlag, Berlin Heidelberg
Yi K, Trulls E, Lepetit V, Fua P (2016) LIFT: Learned Invariant Feature Transform. ECCV 2016. pp
467–483
Ying X, Hu Z (2004) Can we consider central catiodioptric cameras and fisheye cameras within a unified
imaging model. In: Pajdla T, Matas J (eds) Lecture notes in computer science. Computer vision –
ECCV 2004, vol 3021. Springer-Verlag, Berlin Heidelberg, pp 442–455
Yoshikawa T (1984) Analysis and control of robot manipulators with redundancy. In: Brady M, Paul R
(eds) Robotics research: The first international symposium. MIT Press, Cambridge, Massachusetts,
pp 735–747
Zabih R, Woodfill J (1994) Non-parametric local transforms for computing visual correspondence. In:
Ecklundh J-O (ed) Lecture notes in computer science. Computer Vision – ECCV 1994, vol 800.
Springer-Verlag, Berlin Heidelberg, pp 151–158
Zarchan P, Musoff H (2005) Fundamentals of Kalman filtering: A practical approach. Progress in As-
tronautics and Aeronautics, vol 208. American Institute of Aeronautics and Astronautics
Zhan H, Garg R, Weerasekera C, Li K, Agarwal H, Reid I (2018) Unsupervised Learning of Monocular
Depth Estimation and Visual Odometry With Deep Feature Reconstruction. IEEE Conference On
Computer Vision And Pattern Recognition (CVPR).
Zhang Z, Faugeras O, Kohonen T, Hunag TS, Schroeder MR (1992) Three D-dynamic scene analysis:
A stereo based approach. Springer-Verlag, New York
Zhou Q, Park J, Koltun V (2018) Open3D: A Modern Library for 3D Data Processing. ArXiv:1801.09847
Ziegler J, Bender P, Schreiber M, Lategahn H, Strauss T, Stiller C, Thao Dang, Franke U, Appenrodt
N, Keller CG, Kaus E, Herrtwich RG, Rabe C, Pfeiffer D, Lindner F, Stein F, Erbs F, Enzweiler M,
Knöppel C, Hipp J, Haueis M, Trepte M, Brenk C, Tamke A, Ghanaat M, Braun M, Joos A, Fritz
H, Mock H, Hein M, Zeeb E (2014) Making Bertha drive – An autonomous journey on a historic
route. IEEE Intelligent Transportation Systems Magazine 6(2):8–20
 789 A–S

Index of People

A Herschel, William 380, 566

Hesse, Ludwig Otto 746
Ackermann, Rudolph 132
Hough, Paul 534
Asimov, Isaac 3

B I
Babbage, Sir Charles 2 Ibn al-Haytham, Hasan 553
Ball, Sir Robert 73, 78 Ilon, Bengt 143
Bayer, Bryce E. 387
Bayes, Reverend Thomas 214 J
Beer, August 381
Black, Harold 8 Jacobi, Carl Gustav 311
Bode, Henrik 8
Boltzman, Ludwig 379 K
Braitenberg, Valentino 163
Brunel, Isambard Kingdom 335 Kálmán, Rudolf 212, 214
Bryan, George 50 Kepler, Johannes 553

C L
Čapek, Karel 2, 6 Lagrange, Joseph-Louis 350
Cardano, Gerolamo 51 Lambert, Johann Heinrich 405
Chasles, Michel 75 Land, Edwin 402
Chrétien, Henri 427 Laplace, Pierre-Simon 450, 451
Clifford, William 67 Laussedat, Aimé 584
Cook, Captain James 208, 227 Lazzarini, Mario 236
Coriolis, Gaspard-Gustave de 98 Leclerc, Georges-Louis 236
Coulomb, Charles-Augustin de 337 Lie, Sophus 740
Longuet-Higgins, Christopher 654
Lovelace, Countess Ada 2
D
Dalton, John 389 M
Davy, Sir Humphry 380
Delaunay, Boris 186 Markov, Andrey 186
Denavit, Jacques 276 Marr, David 456
Descartes, René 32 Maxwell, James Clerk 385
Devol, George C. Jr. 3, 4 McCarthy, John 8
Dijkstra, Edsger 200 McCulloch, Warren 8
Draper, Charles Stark (Doc) 109, 111, 214 Metropolis, Nicholas 236
Minsky, Marvin 8

E
Edison, Thomas Alva 380
N
Einstein, Albert 97 Newell, Allen 8
Engelberger, Joseph 4, 8 Newton, Sir Isaac 94, 95, 371, 378, 566
Euclid of Alexandria 31, 553 Nyquist, Harold 8, 471
Euler, Leonhard 49, 95, 168, 350
P
G Pitts, Walter 8
Gauss, Carl Friedrich 84, 450 Planck, Max 379
Gibbs, Josiah 84 Plücker, Julius 727
Goetz, Raymond 7 Price, Richard 214
Gray, John McFarlane 335 Ptolemy, Claudius 553

H R
Hall, Edwin 116 Rodrigues, Olinde 56, 84
Hamilton, Sir William Rowan 60, 84
Harrison, John 208
Hartenberg, Richard 276
S
Hering, Karl Ewald 386 Scheinman, Victor 260
790 Index of People

Schmidt, Stanley F. 214 V

Shannon, Claude 8, 471 van Rossum, Guido 14
Simon, Herbert 8 von Goethe, Johann Wolfgang 386
Sobel, Irwin 476 von Helmholtz, Hermann 385
Stefan, Jozef 379 von Kármán, Theodor 276
Swan, Sir Joseph 380 von Neumann, John 236
Voronoi, Georgy Feodosevich 186
T
Tait, Peter 50, 84
W
Tesla, Nikola 7 Wald, George 385
Turing, Alan 8 Walter, William Grey 8, 160, 161
Wien, Wilhelm 379
Wiener, Norbert 8
U
Ulam, Stanislaw 236 Y
Young, Thomas 385
 791 S–C

Index of Functions, Classes and Methods1

A – .landmarks 619
– .optimize 618
angdiff 134, 699
– .plot 616
angvec2r 57
– .setstate 619
– .views 619
B
BagOfWords 529, 530 C
– .contains 529
Camera
– .exemplars 529
– .reset 671
– .occurrence 529
CameraBase 544, 564, 567, 568
– .retrieve 533
CatadioptricCamera 566
– .similarity 531
– .plot_wireframe 567
– .word 529
CentralCamera 544, 547, 552, 555, 564, 567, 568, 577, 594, 604,
– .wordfreq 529
610, 612, 633, 645, 676, 682, 707
– .wwvf 530
– .C 548, 581
BaseFeature2D 517, 524
– .clf 550, 551
Bicycle 140, 156, 198, 212, 216, 221, 225, 228, 238
– .decomposeC 559
– .control 212, 221, 225, 228, 238
– .decomposeE 599, 612
– .curvature_max 198
– .decomposeH 608, 646
– .deriv 156 – .Default 561, 580, 581, 670, 673, 681, 702, 707
– .f 212 – .derivs 618
– .Fv 215 – .disp 604, 610
– .Fx 215 – .E 598, 612
– .plot_xy 215, 221, 226, 228, 239 – .epidist 601
– .q 212 – .estpose 561, 670
– .run 212, 215 – .F 597
– .step 156, 212 – .f 559, 560, 678
BinaryOccupancyGrid 193 – .flowfield 676, 678
– .set 193 – .fov 548
blackbody 379, 381, 395, 401, 403 – .images2C 556
Blobs 499, 500 – .invH 646
– .area 500 – .K 548, 577
– .aspect 500 – .move 612, 613
– .centroid 500 – .plot 556, 560, 579, 606
– .children 502 – .plot_epiline 598, 600, 604, 610
– .circularity 506 – .plot_line2 580
– .filter 501 – .plot_line3 580
– .label_image 502 – .plot_point 549–551, 595, 600, 605–607, 681
– .len 499 – .plot_wireframe 551
– .moments 500 – .points2F 600–603, 610, 634
– .orientation 500 – .points2H 605, 607, 609, 644, 647
– .parent 502 – .pp 681
– .perimeter 505, 506 – .project_line 580, 737
– .perimeter_length 506 – .project_point 544, 545, 547, 549, 550, 555, 561, 578, 599,
– .plot_box 500 600, 614, 670, 673, 674, 690
– .plot_centroid 500, 505 – .ray 612, 613, 616
– .plot_perimeter 505 – .rho 559, 560
– .polar 507 – .visjac_e 690, 691
– .roi 500 – .visjac_l 689
– .umin 500 – .visjac_p 676, 678, 681
Bug2 165 – .visjac_p_polar 701
– .plot 165 cie_primaries 393
– .run 165 circle 689, 690
BundleAdjust 615, 616 cmfrgb 391, 392, 408
– .add_landmark 616 cmfxyz 395
– .add_projection 616 Cobra600 283
– .add_view 616 color2name 396, 401, 488
– .errors 618 colorspace 399
– .getresidual 619 colorspace_convert 397, 398, 408
– .getstate 617 ctraj 107, 287, 289

1 Classes are shown in bold, methods are prefixed by a dot. Other entries are functions.
792 Index of Functions, Classes and Methods

Cuboid 302 – .ikine_LM 294

CurvaturePolyPlanner 195 – .iscollided 302
– .plot 195 – .plot 265, 271, 273, 293, 294
– .query 195 – .rne 338
cylinder 585 – .showgraph 274
– .tool 268
– .URDF_read 270
D ERobot2 263, 268, 308, 317, 321
delta2tr 94 – .fkine 263, 269, 270
DGraph 176 – .plot 263, 269
– .Dict 172 – .showgraph 269
– .highlight_path 198 – .teach 269, 317
– .showgraph 172 esttheta 411
DHLink 276, 296 ET
– .A 276 – * 260, 264, 293, 297
DHRobot 277, 281, 283, 285, 292, 300 – .Rx 293
– .ets 278 – .Ry 260, 264, 297, 338
– .fkine 278, 281–285, 287, 314 – .Rz 260, 264, 293, 297
– .ikine_a 282, 283, 285, 287, 289, 290, 292 – .ty 293, 297
– .ikine_LM 283–285, 290 – .tz 260, 264, 293, 297
– .jacob0 324, 325 ET2 256
– .manipulability 289, 319 – * 256, 258, 259, 262, 268, 278, 280, 308
– .plot 278, 283, 286, 291, 292, 300 – .eta 259
– .teach 278, 310, 325 – .fkine 279, 308
– .twists 300 – .R 256, 258, 259, 262, 268, 278, 280, 308
DiffSteer 156 – .T 259
DistanceTransformPlanner 178, 180, 181 – .tx 256, 258, 259, 262, 268, 278, 280, 308
– .plan 178, 180, 181 ETS 260, 293, 297
– .plot 178, 180, 181 – .fkine 260
– .plot_3d 179 – .n 260
– .query 179–181 – .structure 260
DstarPlanner 182 – .tx 338
– .costmap 182 ETS2 256, 258, 259, 280
– .nexpand 183 – .fkine 257, 258, 280
– .plan 182 – .joints 259
– .query 183 – .n 259
DubinsPlanner 190 – .plot 259
– .plot 190 – .structure 259
– .query 190 – .teach 258
eul2r 49, 50
E eul2tr 65, 76
expm 59
e2h 40, 597
EarthView 428
– .grab 429 F
Edge 170, 769 FeatureMatch 591, 592, 603, 612, 613, 616, 634
– .cost 769 – .correspondence 592
– .endpoints 170, 769 – .distance 592
EKF 215, 217, 221, 225, 226, 228 – .estimate 603, 609, 634, 647
– .get_P 215 – .inliers 603, 609, 610
– .get_Pnorm 216 – .list 592
– .get_t 216 – .match 609
– .landmark 226 – .outliers 609
– .P_est 226 – .plot 592, 603
– .plot_ellipse 216, 221, 228 – .subset 592, 603, 610
– .plot_map 226, 228 Fiducial 577
– .plot_xy 221, 228 – .corners 577
– .run 221, 225, 228 – .draw 578
– .transform 229 – .id 577
– .x_est 226 – .pose 577
ERobot 264, 265, 270, 271, 273, 274, 293, 295, 297, 302, 310–312, findpeaks 770, 771
316–318, 322, 327, 338 findpeaks2d 772
– .addconfiguration 265 findpeaks3d 522
– .base 268 FishEyeCamera 562, 564
– .dynamics 272, 338 – .plot_wireframe 564
– .fkine 265, 268, 293
– .fkine_all 266
 793 C–I
Index of Functions, Classes and Methods

G – .Circles 429
– .clip 440
gauss1d 759
– .close 465, 489
gauss2d 760
– .colorize 433
– .colorspace 398, 399, 486, 488
H – .Constant 442
h2e 40, 559, 597–599, 604 – .convolve 441, 442, 448, 449, 452
HarrisFeature 517, 589 – .copy 459
– .descriptor 590 – .dilate 463
– .distance 590 – .disp 184, 398, 399, 407, 411, 422–424, 426, 428–430, 433–437,
– .Harris_corner_strength 519 440, 442, 443, 448, 449, 456, 457, 459–461, 465, 469–471, 473, 474,
– .p 517 483, 484, 486, 488, 491–495, 500, 502, 505, 508, 513, 517–519,
– .plot 518, 519 521–524, 527–529, 532, 571, 573, 577, 589, 609, 622, 624, 627,
– .strength 517 629–631, 635, 636, 643, 649
– .subset 519 – .distance_transform 185, 467
Histogram 430, 437 – .draw_box 430
– .peaks 431 – .draw_circle 430
– .plot 431, 437 – .draw_line 430
homtrans 579, 605–607 – .DSI_refine 625
Hough 513 – .endpoint 466
– .lines 513 – .erode 463, 465
– .lines_p 513 – .fiducial 577
– .plot_accumulator 513 – .gamma_decode 433, 434, 437
– .plot_lines 513 – .gradients 450
– .plot_lines_p 513 – .Harris 517, 519, 589
– .votes 513 – .hist 430, 484
– .hitormiss 466
– .Hough 513
I – .Hstack 523
IBVS 681, 684, 685 – .humoments 504
– .plot_jcond 682 – .image 422, 423, 446, 452, 633
– .plot_p 682, 686 – .kmeans_color 486, 488
– .plot_pose 682 – .labels_binary 493, 502
– .plot_vel 682 – .labels_graphseg 495
– .run 682, 684, 686, 688 – .labels_MSER 494
– .step 682 – .LUT 435
IBVS_e 692 – .max 422, 430
– .Example 692 – .mean 430
– .run 692 – .median 430
IBVS_l 690 – .meshgrid 472, 558, 573, 628, 633
– .Example 690 – .metadata 611, 645
– .run 690 – .min 422, 430
IBVS_polar 702 – .mono 432
– .plot_p 702 – .morph 461–463
– .plot_pose 702 – .mpq 497
– .run 702 – .ncc 455
IBVS_sph 704 – .nonzero 496
– .run 704 – .normhist 433
idisp 419, 421, 443 – .npixels 435, 459
Image 184, 422, 491 – .open 465
– ~ 437 – .ORB 649
– * 437, 455, 456 – .otsu 484
– ** 450 – .paste 647, 648
– + 436, 437, 440, 450, 455, 456 – .peak2d 457
– - 436, 440, 465 – .plane 398, 399, 423, 437
– / 436 – .polarmatch 507
– .abs 435 – .pyramid 471
– .adaptive_threshold 484 – .Ramp 429
– .affine_warp 474 – .rank 459, 460
– .anaglyph 636 – .Read 398, 411, 422, 423, 430, 432, 437, 442, 446, 455, 456, 461,
– .apply 435 464, 470, 483, 484, 486, 491, 493, 495, 502, 508, 510, 513, 517, 519,
– .apply2 436, 450 520, 523, 525, 527, 571, 577, 589, 609, 620, 634, 643
– .blobs 499, 501–503 – .rectify_homographies 634
– .canny 451, 513 – .red 423
– .centre 474, 556 – .replicate 471
– .choose 437, 492, 627, 629 – .roi 469, 470
794 Index of Functions, Classes and Methods

– .rotate 475, 500 – .plot 191, 192, 194

– .rotate_spherical 573 – .query 192, 194
– .sad 455 LatticeStatus 192
– .samesize 437 Line3 301, 612, 613, 737
– .scalespace 520, 523 – | 727
– .shape 422 – ^ 727
– .SIFT 524, 527, 589, 591, 609, 634, 647 – .closest_to_line 612, 613
– .similarity 456 – .closest_to_point 727
– .Sin 429 – .intersect_plane 727
– .smooth 443, 471 – .Join 580, 726
– .sqrt 435, 450 – .plot 65, 76, 301, 726
– .Squares 429, 467, 512 – .point 726
– .ssd 455 – .side 727
– .stats 422, 423, 430, 433 – .skew 726, 737
– .std 430 – .v 726
– .stdisp 620, 634 – .w 580, 726
– .stereo_BM 630 Link 270
– .stereo_SGBM 631, 635 Link2 262, 263, 268
– .stereo_simple 622, 623 – .A 264
– .stretch 433 – .children 264, 268
– .sum 496 – .ets 264
– .thin 184, 466 – .isprismatic 264
– .Tile 527 – .isrevolute 264
– .to 432 – .jindex 264
– .triplepoint 185, 466 – .parent 263
– .tristim2cc 437 – .qlim 264
– .upq 497, 498 loadspectrum 381–384, 403–405, 408, 414
– .uspan 450 luminos 386
– .view1d 459, 627
– .vspan 450
– .warp 473, 558, 571, 573
M
– .warp_perspective 634, 645, 648 mkcube 550, 551, 555, 561, 564, 567, 568
– .window 459 mkgrid 549, 605, 670, 681, 704
– .zerocross 452 mpq_point 733
– .Zeros 430, 647 mstraj 102, 103, 291, 294
– .zncc 455, 456 mtraj 101, 104, 285
– .zsad 455, 456 mvtb_load_matfile 770, 772
– .zssd 455, 456
ImageCollection 424, 531, 556, 647
IRB140 277, 300
N
iread 418, 420 name2color 396, 491
numjac 748
numpy
J – .angle 56
jsingu 316 – .argmax 499, 770, 772
jtraj 286, 290, 291, 349 – .argsort 531
– .cross 95, 119
– .delete 578
K – .diff 447, 652
Kernel – .finfo.eps 71
– .Box 442, 445 – .flatten 681
– .Circle 445, 464, 489 – .linalg.cond 317
– .DGauss 449, 450 – .linalg.det 35, 70, 71, 93, 316, 581
– .DoG 453 – .linalg.eig 55, 318, 356, 498, 730
– .Gauss 443, 449 – .linalg.inv 40, 95, 316–318, 322, 355, 363, 400, 579, 581, 733
– .Laplace 452 – .linalg.lstsq 329, 690
– .LoG 452 – .linalg.matrix_rank 316, 323, 597, 600
– .linalg.norm 280, 321–323, 599, 607, 733, 747
– .linalg.pinv 321, 323, 681
L – .linalg.svd 445
lambda2rg 392–394 – .linspace 99, 100, 351, 352
lambda2xy 395, 401, 403, 405 – .loadtext 652
LandmarkMap 218, 221, 225, 228, 238 – .meshgrid 351, 352
– .landmark 219 – .outer 445
– .plot 218, 221, 226, 228, 239 – .random.choice 178, 459, 602
LatticePlanner 191–193 – .random.default_rng 733
– .plan 191–193 – .random.normal 759, 766
 795 I–R
Index of Functions, Classes and Methods

– .random.rand 187, 490 – .iscollision 197

– .random.seed 602 – .plot 197
– .random.unifor 186 PoseGraph 233, 242
– .random.uniform 600 – .optimize 233
– .ravel 772 – .plot 233
– .uint8 436 – .scan 243
– .unravel_index 772 – .scanmap 244
– .scanmatch 243
– .scanxy 243
O – .time 243
oa2r 55 PR2 274
OccupancyGrid 244 PRMPlanner 185, 186
– .plot 244 – .plan 185, 186
ORBFeature – .plot 186, 187
– .plot 649 – .query 187
Puma560 281, 292, 341, 349, 351
P
Panda 265, 285, 302 Q
ParticleFilter 238 quintic 99, 285
– .get_std 239
– .plot_pdf 239
– .plot_xy 239
R
– .run 238 RandomPath 212, 225, 238
PBVS 670 RangeBearingSensor 219, 221, 223, 225, 226, 228, 238, 249
– .plot_p 671 – .h 221
– .plot_pose 671 – .Hp 225
– .plot_vel 671 – .Hw 221
– .run 671, 686 – .Hx 221
plot_arrow 731 – .reading 219
plot_box 496 ReedsSheppPlanner 190
plot_chromaticity_diagram 395, 409, 487 – .plot 190
plot_circle 457, 522 – .query 190
plot_ellipse 498, 730, 732, 734, 761 RevoluteDH 276
plot_ellipsoid 318, 355 RevoluteMDH 276, 299
plot_homline 580, 736 Robot 288, 355
plot_labelbox 509 – .accel 357
plot_point 39, 40, 168, 393, 394, 457, 487, 490, 497, 609, 644 – .base 351
plot_polygon 644 – .coriolis 353
plot_spectral_locus 392 – .coriolis_x 363
plot_sphere 560, 572, 579, 595, 606 – .friction 353
plotvol2 38, 40 – .gravity 350
plotvol3 560, 572, 595, 606, 613 – .gravload 350, 351, 354
PoELink 301 – .ikine 283
PoERevolute 301 – .ikine_LM 283, 327
PoERobot 301, 328 – .ikine_LMS 322
– .fkine 301 – .inertia 352, 354, 355
– .jacob0 328 – .inertia_x 363
PointCloud 613, 614, 633 – .jacob0 310, 316–318, 321, 322, 355, 363
– * 614, 643 – .jacob0_analytical 312
– + 643 – .jacob0_dot 361
– .disp 613, 614, 633, 640, 643 – .jacobe 311
– .downsample_random 643 – .jacobm 327
– .downsample_voxel 640 – .jointdynamics 341
– .ICP 643 – .manipulability 319, 356
– .normals 640 – .nofriction 357
– .paint 643 – .payload 354
– .Read 640 – .rne 349, 355
– .remove_outlier 614 – .teach 330
– .segment_plane 641 – .vellipse 319
– .transform 613, 614, 633, 643 rot2 34, 35, 38
Polygon2 197 rotvelxform 312
– .area 699 rotx 47, 48, 57, 58, 80, 92
– .moment 506 roty 47–49
– .transformed 197 rotz 47, 49
PolygonMap 197 rpy2r 51, 55, 70
– .add 197 rpy2tr 59
796 Index of Functions, Classes and Methods

RRTPlanner 198 – .descriptor 591

– .iscollision 198 – .distance 591
– .plan 198 – .estimate 610
– .plot 198, 199 – .id 528
– .qrandom 198 – .match 591–593, 610, 634, 647
– .query 198 – .plot 524, 528
rtb_load_jsonfile 168, 291 – .scale 524
rtb_load_matfile 164, 180 – .sort 527
– .support 527
– .table 527
S SIFTPointFeature
scipy – .match 634
– .cluster.vq.kmeans2 490 skew 36, 58, 92, 742
– .linalg.expm 35, 41, 42, 58, 63, 64 skewa 41, 42, 63, 64
– .linalg.logm 35, 41, 57, 63 SO2 80, 102
– .linalg.null_space 323, 559, 597, 598, 604, 678, 750 – * 102
– .linalg.spsolve 750, 752 SO3 80, 104
– .linalg.sqrtm 732 – * 104, 573
– .optimize.minimize 280 – .A 80
– .signal.convolve2d 449 – .animate 104
– .spatial.Voronoi 186 – .eul 81
– .stats.distributions.chi2 761 – .printline 81
– .stats.linregress 602 – .RPY 80, 104
SE2 80, 81, 232, 257, 263, 279, 300 – .rpy 81, 104
– * 81, 232, 257, 258, 300 – .Rx 80, 82
– .inv 81, 232 – .Ry 104, 573
– .plot 81 – .Rz 80, 104, 573
– .printline 81, 258, 280 sphere 585
– .R 81 SphericalCamera 568, 704, 710
– .Rot 257, 258 – .plot_wireframe 568
– .t 81, 279 – .visjac_p 704
– .Tx 257, 258 sympy
SE3 80, 90, 105, 107, 266, 281–285, 287, 289, 290, 292, 294, 301, – .det 35
594, 616, 670, 686 – .Eq 581
– * 90, 105, 268, 284, 285, 288, 290, 292, 295, 300, 322, 550, 551, – .expand_trig 279
555, 560, 561, 578, 581, 599, 605, 608, 643, 670, 681, 686, 688, 702, – .jacobian 232, 309, 749
704, 707 – .Matrix 35, 232, 309, 581, 748
– .Ad 91, 96 – .MatrixSymbol 748
– .animate 105 – .plot_implicit 581
– .Delta 681 – .simplify 35, 232
– .interp 105, 107 – .solve 279
– .interp1 670 – .Symbol 35
– .inv 96, 559, 599, 608, 643, 670 – .symbols 232, 278, 279, 308, 338, 581
– .jacob 90 – .trigsimp 279
– .OA 285, 288, 292
– @ 71, 670, 681
– .plot 284, 560
T
– .printline 265, 266, 281–285, 301, 560, 561, 599, 608, 612, 619, t2r 62
643, 646, 670 torch
– .prod 300 – .argmax 491
– .RPY 105, 284, 551, 555, 561 – .stack 491, 508
– .rpy 101, 106, 287 torchvision
– .Rx 90, 268, 285, 290, 351, 578, 581, 605, 614, 633, 674, 707 – .models.detection.fasterrcnn_resnet50_fpn 508
– .Ry 322, 550, 594, 605 – .models.segmentation.fcn_resnet50 491
– .Rz 90, 268, 295, 643, 681, 686, 688, 702, 704 – .transforms.Compose 491
– .t 101, 105, 287, 314, 559 – .transforms.Normalize 491
– .Trans 268, 284, 285, 288, 290, 292, 295, 322, 550, 551, 555, 561, – .transforms.ToTensor 508
581, 594, 605, 616, 643, 670, 681, 686, 704, 707 tr2angvec 55, 93
– .Tx 90, 282, 301, 302, 674 tr2delta 94
– .Tz 268, 551, 578, 670, 674, 681, 686, 688, 702, 704 tr2eul 49, 50
Sensor 219 tr2rpy 51
shadow_invariant 411 Trajectory 102–104, 285, 286, 291
ShapeCollision 302 – .plot 99–101
– .base 302 – .q 103, 104, 107, 286, 287, 291
SIFTFeature 524, 527, 589, 591, 609, 634, 647 – .qd 100
– + 527 tranimate 47
 797 R–Z
Index of Functions, Classes and Methods

transl 62, 63, 65, 76 Unicycle 156

transl2 38–40, 43 UnitQuaternion 59, 61, 95, 104, 109, 118, 119
trapezoidal 100, 101, 107, 285 – * 95, 119
trexp 58, 59, 75, 92 – .angdist 118, 120
tripleangledemo 52 – .animate 104, 105, 110
tristim2cc 394, 401, 409 – .append 110, 118, 119
trlog 57 – .dot 89
trnorm 71 – .dotb 89
trot2 38, 40, 43 – .EulerVec 95, 110, 118, 119
trotx 62–64 – .interp 104, 105
trplot 47, 62, 65 – .inv 60, 119
trplot2 34, 38–40 – @ 71, 110, 118, 119
Twist2 42, 43 – .plot 60, 95
– .exp 42, 43, 474 – .R 60, 95
– .pole 42, 43 – .rpy 110
– .SE2 300 – .Rx 70
– .UnitPrismatic 43 – .Rz 70, 104
– .UnitRevolute 42, 300, 474 – .unit 71
– .w 43
UR5 271
Twist3 65, 76, 300
UVertex 169, 768
– .exp 64, 65, 75, 76
– .adjacent 769
– .line 65, 76, 301
– .connect 200
– .pitch 66
– .edges 170, 769
– .pole 66
– .printline 76
– .S 75 V
– .SE3 300
VehicleBase 156, 211
– .se3 75
vex 35, 36, 57, 58, 742
– .theta 66, 76
vexa 41, 42, 63, 64
– .unit 76
– .UnitPrismatic 65 VideoCamera 424, 426, 427
– .UnitRevolute 64, 65, 75 – .grab 426
– .v 75 – .release 424
– .w 66, 75 VideoFile 426, 427, 440
twistdemo 77 – .shape 426
VisualServo 671, 682

U
W
UGraph 168, 200, 768
– .add_edge 168, 175, 768 WebCam 427
– .add_vertex 168, 175, 200, 768 – .grab 428
– .average_degree 170 wrap_0_2pi 699
– .closest 769 wrap_mpi_pi 134, 699
– .degree 169
– .distance 200
– .highlight_path 170
X
– .highlight_vertex 174 xplot 103, 106, 110, 118, 120, 286, 287, 289, 314, 357
– .n 169
– .nc 170
– .ne 169
Y
– .neighbors 169 YuMi 273
– .path_Astar 174–176, 769
– .path_BFS 170
– .path_UCS 172
Z
– .plot 169, 170, 174, 769 ZipArchive 424, 649
 799 B–Z
Index of Models

Index of Models2

B L
braitenberg 162, 163 lanechange 132, 133

C O
computed-torque 360 opspace 366

D P
driveconfig 140 ploop 346, 347
driveline 136 ploop-test 346, 347
drivepoint 134, 135
drivepursuit 137 Q
quadrotor 150, 151
F
feedforward 359 R
RRMC 313, 314
I RRMC2 315
IBVS 683
IBVS-arm 705, 706 S
IBVS-driveconfig 709 SEA 368, 369
IBVS-drivepoint 707
IBVS-holonomic 708
IBVS-nonholonomic 709 V
IBVS-partitioned 699, 700 vloop 342, 371, 372
IBVS-quadrotor 710, 711 vloop-test 342, 343

J Z
jointspace 288 zerotorque 357

2 These block diagram models are included in the rvc3python package in the folder models. To run them requires the bdsim package.
 801 A

General Index

 (relative pose) 24 – iterated closest point (ICP) 247

3D reconstruction 588, 631 – k-means 486, 529
– Levenberg-Marquardt (LM) 327, 753
– maximally stable extremal region (MSER) 494, 509
A – Newton-Euler 348
A* search 174 – Probabilistic Roadmap Method (PRM) 185
Abelian group 28 – Random Sampling and Consensus (RANSAC) 601
absorption – resolved-rate motion control 313
– coefficient 381 – SIFT 523, 591
– color change 404 – skeletonization 185
– spectrum 381, 405 – stereo matching 621
abstract pose  24 – thinning 184, 185
acceleration 95, 110, 112, 117 – uniform-cost search (UCS) 172
– angular 95 aliasing
– centripetal 97 – anti-, graphics 430
– Coriolis 97, 98, 122 – spatial 471, 625, 630
– Euler 97 ambiguity ratio 625, 660, 770
– gravitational 97, 110, 112 ampullae 112
– inertial 112 anaglyph 47, 636
– proper 112 analytical Jacobian 312, 362
– sensor 112, 117 anamorphic lens 427
– specific 112 angle
accelerometer 53, 110, 111, 118 – azimuth 568, 703
– triaxial 112, 117, 118 – colatitude 568, 703
accommodation 541 – declination 114
Ackermann steering 131 – difference 134
active rotation 68 – dip 114
actuator 334 – elevation 208
– brushed motor 334 – heading 114
– brushless motor 334 – inclination 114
– electro-hydraulic 334 – longitude 703
– pneumatic 334 – pitch 50
– series-elastic (SEA) 367–369 – polar 701
– stepper motor 334 – roll 50
added mass 147 – solid 379, 388, 549
adjoint – steered wheel 131, 132
– logarithm 744 – steering wheel 131
– matrix 91, 96, 300, 328, 728, 744 – subtraction 134
adjugate 720 – yaw 50, 114, 130
aerial robot 146 angle-axis representation 55, 59, 91, 617
affine angles
– camera 581 – bank, attitude, heading 50
– reconstruction 620 – Cardan 45, 50
– space 738 – Euler 45, 48, 52, 53, 101
– transformation 473, 738 – rate of change 312
AHRS (Attitude and Heading Reference System) 118 – nautical 50
aircraft – roll-pitch-yaw 101, 106
– configuration space 153 – rate of change 312
– fixed-wing 154 – XYZ 50, 51, 287, 288
– quadrotor 146 – YXZ 547, 599, 672, 682
Airy pattern 442 – ZYX 50
albedo 382 – rotation 53, 58
algebraic topology 71 – Tait-Bryan 50
algorithm angular
– A* 174 – acceleration 95, 353, 361
– breadth-first search (BFS) 170 – distance 704
– Bresenham 244 – momentum 95, 108
– bug2 165 – uncertainty 216
– D* 182 – velocity 88, 95, 97, 107, 108, 109, 119, 210, 312, 763
– Dijkstra search 199 – velocity, of a point 89
– FastSLAM 246 anthropomorphic 203, 261
– graphcuts 495, 509 anti-aliasing
802 General Index

– filter 471, 474 B

– graphics 430 back
anti-symmetric matrix, see skew-symmetric matrix 720 – EMF 346
aperture – end, SLAM 235
– lens 543, 553 bag of words 528
– pinhole 540, 574 barometric pressure 118
– stop 543 barrel distortion 552
Apollo base wrench 355
– 13 52, 54 basis vector 32, 33, 34, 44, 46, 717
– Lunar Module 53, 109 Bayer
application – filter 387
– character recognition 525 – pattern 387
– chroma keying 436 Beer’s law 381
– color spaces 408 behavior-based robot 164
– fiducial marker 576 Beidou (satellite navigation system) 209
– finding Wally 456 Bézier spline 195
– holonomic robot visual servo 707 bicycle model 130, 139, 708
– image retrieval 526 – visual servo 707
– inertial navigation 107 bicycle motion model 130
– lidar-based map building 244 binarization 482
– lidar-based odometry 242 binary
– mosaicing 646 – classification 482
– motion detection 439 – image 433
– operational-space control 366 bi-quaternion (see dual quaternion) 67
– perspective correction 643 blackbody radiator 379, 399, 401, 410
– planar homography 578 blade flapping 148
– quadraped walking robot 292 blend 100, 102
– resolved-rate motion control 313 blob see region feature
– series-elastic actuator 367 – coloring 493
– shadow removal 410 – labeling 493
– visual odometry 649 block diagram 288
body-fixed frame 52, 78, 97, 107
– visual servo
Boltzmann constant 379
– mobile robot 707
bootstrap filter 237
– nonholonomic mobile robot 707
bounding box 496
– quadrotor 709
box filter 442
– robot manipulator 705
Braitenberg vehicle 162, 163
– writing on a surface 291
Bresenham algorithm 244
approach vector 54, 55, 284, 285, 292
Brockett’s theorem 154, 712
April tag 576
brushfire planner 181
arithmetic B-spline 195
– error 70 Buffon’s needle problem 236
– precision 71 bug algorithm 164
artificial intelligence 8, 11, 19 bundle adjustment 248, 614, 615, 617–619, 650
ArUco marker 576 buoyancy force 147
aspect ratio
– blob 498
– image 427 C
– sensor 546 calibration
assorted pixel array 387 – camera 547, 553
astigmatism 551 – color 404
ASV (Autonomous Surface Vehicle) 128 camera 230
Attitude and Heading Reference System (AHRS) 118 – affine 581, 620
augmented skew-symmetric matrix 42, 64, 72, 74 – aperture 425
AUS (Autonomous Underwater System) 128 – array 574, 576
autocorrelation matrix 516 – aspect ratio 427
automata 164 – baseline 620, 633
AUV (Autonomous Underwater Vehicle) 128 – black level 455
axis – calibration 553, 559, 583, 645, 669
– of motion 101 – catadioptric 564, 566, 567, 570, 584, 698, 703
– of rotation 88, 95 – caustic 566
– optical 55, 540, 634, 643, 645, 673, 688 – CCD 387
– screw 73 – center 553, 555, 575
azimuth angle 568, 703 – central 541, 566, 572, 574
– central perspective 541, 544
– depth of field 425, 543
 803 A–C
General Index

– distortion 552 – coordinate system 32

– DSLR 426 – geometry 31
– dynamic range 426 – motion 105, 285, 287
– exposure – plane 31
– time 425, 543 – product 717
– value 425, 590 catadioptric camera 564
– extreme low light 425 caustic 566, 574
– extrinsic parameters 548, 553, 582, 620 CCD sensor 425
– field of view 549 celestial navigation 208
– fisheye 563, 570, 703 census
– f-number 425 – metric 590
– focal length 424, 425, 541 – transform 458, 630
– frame 541, 542, 544 center
– high dynamic range 387 – of gravity 337
– hyperspectral 412 – of gravity law (color) 393
– image plane 541 – of mass 266, 272, 337, 349, 354, 355
– image plane, discrete 545 central
– infrared 412, 638 – imaging 566, 573
– intrinsic parameters 547, 548, 552, 553, 561, 577, 597, 598, 608, – moments 497
610, 612, 620, 670, 676, 681, 684 – perspective model 541
– lens 540 centrifugal
– light-field 574, 584 – acceleration 110
– location determination problem 560 – force 97
– low-light 575 centripetal
– matrix 544, 547–549, 555, 559 – acceleration 97
– matrix, decomposition 559 – coupling 348
– motion 610, 612, 645, 649 chamfer matching 469
– multispectral 387 character recognition 525
– noncentral 566 characteristic scale 522
– nonperspective 561, 581, 703 charge well 425, 426
– omnidirectional 549 Chasles’ theorem 73
– optical axis 540, 643, 686, 699, 701, 703 Cholesky decomposition 722
– orientation 559 chroma keying 436
– panoramic 549 chromatic aberration 551
– parameters 548, 620 chromaticity 392, 399, 408
– perspective 540, 544, 561, 565, 569, 574, 620, 698, 705 – coordinates 392, 395
– photosite 612 – diagram 392, 395
– pinhole 540, 574 – plane 393
– plenoptic 574 – white 401
– pose 247, 548, 610, 650, 669 chromophore 385
– pose estimation 670 CIE 393
– principal point 541, 675, 676, 681, 701 – 1931 standard primary colors 389, 393
– quantum efficiency 425 – CIELAB color space 397
– resectioning 583 – CIELUV color space 399
– retreat 686, 698 – XYZ color space 393
– RGBD 638 circle 736
– sensor 386, 410, 411, 545 circle of confusion 543
– shutter circularity 506
– global 425 city-block distance 177
– rolling 425 classification 482
– SLR 427 – binary 482
– spherical 567, 569, 703, 704, 709, 712 – color 486
– stereo 10, 620, 621, 631, 634 – gray-level 482, 483
– SWIR 412 – semantic 491
– thermal 412 clothoid 131, 195
– thermographic 412 clustering (see k-means clustering) 490
– ultraviolet 412 CMOS sensor 425, 550
– unified 569, 570 coarse-to-fine strategy 471
– velocity 674–676, 679, 680, 684, 686, 690–692 COCO dataset 508
– verged 600 colatitude 568, 703
– video 408 collineation 738
– wide-angle 561, 584, 678, 703 color
Canny edge detector 450 – apparent change 404
canonical image coordinates 542 – blindness 389
car 154 – calibration 404
Cartesian – constancy 378
804 General Index

– filter 387, 389 connected components 492, 495, 631

– gamut 393 – graph 769
– hue 392 connectivity analysis 493
– image 420 consistency, left-right check 593, 627
– matching function 390, 392, 395, 409 constant
– measuring 387 – Boltzmann 379
– names 395 – Planck 379
– plane 421, 442 constraint
– primary 389, 390 – epipolar 594, 598, 622, 650, 651
– reproduction 389, 392 – holonomic 78, 79, 154
– saturation 392 – nonholonomic 132, 143, 154, 189
– space 396, 399 – nonintegrable 154
– CIELAB 397 – rolling 154
– CIELUV 399 continuity 190
– HSV 408 C 0 continuity 190
– L a b 408 C 1 continuity 190
– L u v 399 C 2 continuity 190
– opponent 399 contour (see perimeter) 505
– perceptually uniform 399 control
– rgY 397 – admittance 365
– xyY 397, 408 – altitude 151
– XYZ 393, 395, 408, 409 – application
– YCB CR 408 – car-like vehicle 133
– YUV 399, 408 – computed-torque 359
– spectral 392, 398 – quadrotor 150
– temperature 401, 411 – robot joint 334
Color Checker 409 – series-elastic actuator 367
colored point cloud 614, 633 – torque feedforward 358
colorimetry 393, 412 – attitude 150
column space 723 – compliance 365
CoM (see also center of mass) 337 – computed-torque 359
Commission Internationale de l’Eclairage, see CIE 393 – feedback linearization 364
compass 116, 139, 210, 222 – feedforward 151, 345, 346
– heading 206 – hierarchical 150
compound lens 541 – impedance 364
compression – integral 345, 371
– gamma 407 – inverse-dynamic 359
– image 427, 516 – loop, nested 334
– JPEG 421 – operational space 366, 367
– lossless 420 – proportional (P) 133, 136, 149, 342
– lossy 421 – proportional-derivative (PD) 149
computed torque control 359 – proportional-integral (PI) 345
Concurrent Mapping and Localization (CML) 227 – proportional-integral-derivative (PID) 151, 346
condition number (see matrix condition number) 316 – resolved-rate motion 313, 329
cone 737 – shared 7
cone cell (see human eye) 384 – traded 7
confidence – velocity 133, 342, 346
– ellipse 216 – vision-based 668
– test 223 control:mobile robot 133
configuration convolution 441, 449
– change 290 – kernel 441
– kinematic 282, 314 – properties 442
– space 77, 78, 130, 152, 154, 259, 283, 285, 309 coordinate frame 32, 44, 244
– aircraft 146, 152 – acceleration 95
– car 153 – axes 26, 44
– helicopter 153 – basis vectors 27, 32, 44
– hovercraft 152 – body-fixed 27, 52, 97, 107, 147
– train 152 – inertial 95, 97, 107
– underwater robot 152 – link 261, 266, 274
– zero-angle 274, 296 – noninertial 97
conformal transformation 543 – orientation 27
conics 543, 569, 581, 736 – origin 26
conjugate – right-handed 26, 44
– point 594, 596, 598–600, 606, 610, 621 – translation 27
– quaternion 61 – world reference 27
conjugation 41, 723 coordinate vector 27, 28, 32, 44
 805 C–D
General Index

coordinates – QR 722
– Cartesian 31, 32, 44 – RQ 559
– Euclidean 734 – singular value 445, 642, 724, 750
– exponential 57, 69, 101, 311, 742 – spectral 723
– generalized 78, 259, 275 deep learning 491
– homogeneous 38, 734 – COCO dataset 508
– image-plane 675 – Faster R-CNN 508
– joint 308 – object detection 508
– nonhomogeneous 38 – PASCAL VOC dataset 491
– normalized 675 – PyTorch 491
– normalized, image 542 – ResNet-50 491
– Plücker 726 degrees of freedom 52, 78, 101, 146, 152, 154, 259, 282, 289, 310, 316,
– retinal, image 542 318, 674, 695, 698, 711
Coriolis Denavit-Hartenberg
– acceleration 97, 98, 122 – constraints 274, 296
– coupling 348, 353 – modified notation 298
– matrix 353 – notation 274, 299
corner point (see point feature) 514 – parameters 267, 275, 296
Cornu spiral 195 – standard vs modified 299
correlation 441 dense stereo, see stereo 620
correspondence 637, 643, 650 depth of field 543
– inliers 601, 602, 610, 651 derivative
– outliers 601, 602, 609 – SO.3/ 88, 89
– point 600, 651 – Euler angles 312
– point feature 589, 590, 622, 680, 690 – exponential coordinates 312
– problem 588, 690 – of Gaussian 449, 451, 516
– putative 592, 602 – orientation 88
cost map 182 – pose 89
Coulomb friction 336, 340 – quaternion 89
covariance matrix 210, 211, 216, 217, 219, 224, 226, 240, 760 – roll-pitch-yaw angles 151
– correlation 210, 760, 765 derotation, optical flow 685
– ellipse 217 detector
– extending 225 – Harris 517, 589
– transformation 764, 766 – SIFT 589
crack code 505 – zero crossing 452
crawl gait 293 determinant 518, 723
cropping 469 – of Hessian 518
cross product 58, 71, 718 dichromat 384
C-space (see configuration space) 78 dichromatic reflection 406
curvature 194, 520 difference of Gaussian (DoG) 453
– image 516 differential
– maximum 189 – kinematics 308
– path 189, 195 – steering 140
– perimeter 507 diopter (see also focal length) 541
– polynomial 195 dip angle 114
– principal 516 Dirac function 378, 410
cybernetics 4, 8, 161, 162, 202 direct linear transform 554, 583
cylinder 737 direction cosine matrix (DCM) 69
disparity 621, 627
– image 622
D – space image (DSI) 623
D* planning 182 displacement, spatial 94
D65 white 400, 401, 408 distance
d’Alembert, force 97 – angular 704
Daltonism 389 – city-block 177
damped inverse 320, 753 – Euclidean 177, 178
data association 207, 208, 223, 589, 600, 602 – Hamming 458
DCM (Direction Cosine Matrix) 69 – L1 177, 717
dead reckoning 206, 210 – L2 177, 717, 718
declination – Mahalanobis 223, 722, 760
– angle 114, 115 – Manhattan 177, 178, 718
– magnetic 114 – orientation 69
decomposition – p-norm 717
– camera matrix 559 – transform 177, 185, 467, 468
– essential matrix 599 – triangle inequality 70
– homography matrix 608 distortion
806 General Index

– barrel 552 – equation 690

– hard-iron 117 – equivalent 498, 729
– keystone 643 – error 217, 221
– lens 551, 619, 634 – fitting to
– perspective 588, 643 – perimeter 734
– pincushion 552 – points 732
– soft-iron 117 – inertia of 733
distribution – latus rectum 569
– bimodal 484 – major axis 729, 731
– Gaussian 759, 762, 763, 765 – minor axis 729
– non-Gaussian 766 – orientation 731
– Poisson 425 – parameter 690
– von Mises 211 – polynomial form 730
– 2 761 – radii 729
DoG (difference of Gaussian) 453 – uncertainty 216
dot product 718 ellipsoid 580, 728, 737, 760
double mapping 55, 59, 70, 617 – acceleration 355
drag – equivalent 641
– hydrodynamic 147 – force 325
drag, see also friction – principal axes 318
– aerodynamic 148 – velocity 317
drone 146 – volume 319, 328
DSI (disparity space image) 623 – wrench 325
dual encoder
– number 67 – joint 340
– quaternion 67 – wheel 242
Dubins path 189 encoding, gamma 434
dynamic range 426 end effector 254
dynamics – coordinate frame 311
– error 359, 360, 363 – velocity 309
– forward 149, 334, 356, 357 – wrench 324, 325
– inverse 348 ENU (east-north-up) 107
– quadrotor 151 Eötvös, effect 122
– rigid-body 95, 348, 358 ephemeris 208
epipolar
– aligned image 634
E – constraint 594, 598, 622, 650, 651
Earth – line 594, 596, 599, 601, 604, 610, 620
– diameter 110 – plane 594
– gravity 110 epipole 595, 597
– magnetic field 115 equal-energy white 401
– shape 110 equation
east-north-up (ENU) 107 – differential 71
eccentricity 569, 730 – ellipse 680
edge – lens maker’s 541
– detector 450, 459, 476 – motion, of
– graph, of 755, 768 – bicycle 131
– preserving filter 460 – quadrotor 148
edgels 505 – rigid-body 348, 356
EGNOS 209 – unicycle 142
eigenvalue 55, 56, 217, 319, 356, 498, 516, 641, 721 – of rotational motion (Euler’s) 95
eigenvector 55, 56, 498, 641, 721 – optical flow 676, 703
EKF SLAM 226 – Planck radiation 379
elasticity – prediction 224
– joint 367 – Rodrigues’ 56, 59, 75
– link 368 – rotational motion (Euler’s) 148
elementary transformation sequence 256, 263, 275, 297, 299 equations
ellipse 542, 580, 688, 690, 728, 736, 760 – linear
– area 217 – homogeneous 750
– axes 730 – nonhomogeneous 750
– principal 498, 680 – nonlinear 751
– canonical 729 – over determined 320
– confidence 226, 228 – sparse nonlinear 618, 754
– covariance 226, 761 – under determined 321
– drawing 731 equiangular mirror 565
– eccentricity 569, 730 equivalence principle 97
 807 D–F
General Index

equivalent F
– ellipse 498, 729
FAST detector 591
– ellipsoid 641
Faster R-CNN network 508
error FastSLAM 240
– cumulative 230 feature 480
– ellipse 217 – depth 684
– landmark 240 – descriptor 524, 590
– reprojection 619 – detector (see point feature detector) 523
essential matrix 598, 599, 610, 612 – ellipse 676, 690
estimation – extraction 13
– camera matrix 554 – Harris corner 517
– camera pose 560, 670, 709 – line 510, 688
– fundamental matrix 600 – map 221, 228
– homography 605 – point 514
– Monte-Carlo 214, 236, 246 – point (see point feature) 514
– Structure and Motion (SaM) 615, 712 – region 482
– vehicle pose 209 – region (see region feature) 482
estimator – scale 523
– maximum likelihood 754 – scale space 522, 523
– robust 754 – sensitivity matrix, see Jacobian, image 674
Euclidean – SIFT 523
– coordinates 734 – vector 482, 590
– distance 31, 178, 769 feedforward control 151, 345, 346, 358
– geometry 31, 725 fibre-optic gyroscope (FOG) 108
– homography 608, 646 fictitious force 97, 112
– length 718 fiducial marker 126, 223, 576
– plane 31, 735 field
– point 735 – geomagnetic 114
– space 78, 738 – magnetic, flux density 117
– transformation 738 – of view 548, 549, 561
Euler – robot 4, 128
– acceleration 97 – vector 747
– angles 48, 49, 311 file type
– rate of change 312 – COLLADA 271
– singularity 50 – image 420, 421
– parameters 57, 59 – JFIF 407
– -Rodrigues formula 57 – LAGO 233
– rotation theorem 48 – PCD 658
– rotational motion equation 95, 148, 348 – PLY 640
– spiral 195 – STL 271
– vector 57, 95, 742 – TORO 233
EV (exposure value) 425 – URDF 270
EXIF – video 427
– file format 421 fill factor 425, 550
– metadata 407 filter
explicit complementary filter 119 – anti-aliasing 471, 474
exponential – bandpass 449
– coordinates 59, 69, 311, 312 – Bayer 387
– rate of change 312 – box 442
– mapping 71, 73 – coefficients 441
– matrix 35, 57, 58, 63, 71, 74 – color 387
– product of (PoE) 299 – complementary 119
exposure – edge preserving 460
– time 425 – extended Kalman (EKF) 119, 765
– value 455 – high-pass 449
– value (EV) 425, 426 – Kalman 121, 212, 220, 223, 229, 235, 240, 245, 247, 763
extended Kalman filter (EKF), see Kalman filter 212 – low-pass 449, 471
exteroception 8 – median 460
extromission theory 378 – particle 236, 237, 238, 240, 241
eye – rank 459
– evolution of 11 – separable 445
– see also human eye 383 – spatial 441
– spots 11 fisheye lens 562
eye-in-hand visual servo, see visual servo 668 flux
– density, magnetic 114
– luminous 386, 388
808 General Index

– visual 378 – multivariate 760

f-number 425, 543 – noise 555, 561, 763, 765
focal – probability 760
– length 541 – properties 445, 523
– point 541, 648 – random variables 759
FOG (fibre-optic gyroscope) 108 – smoothing 495, 516
force 95, 324 – width 449
– apparant 97 gearbox 339, 340
– d’Alembert 97 – ratio 339, 350
– ellipsoid 325, 355 generalized
– fictitious 97, 112 – coordinates 78, 259, 275
– inertial 97 – force 324, 325, 326, 348
– pseudo 97 – matrix inverse 723
foreshortening 475, 543, 643, 644 – velocity 317
forward generator matrix 741, 743
– dynamics 356 geomagnetic field 114
– kinematics 255, 257, 300, 310 geometric
Fourier transform 507 – distortion 551
fovea 385 – invariant 739
frame (see coordinate frame) 26 – Jacobian 310
Freeman chain code 505, 534 – transformation 739
Fresnel – affine 738
– integral 195 – homothety 738
– reflection 406 – reflection 738
friction 336 – rotation 738
– aerodynamic 148 – scaling 738
– Coulomb 336, 340 – shear mapping 738
– hydrodynamic 147 – translation 738
– joint 348, 353 geometry
– Stribeck 336 – 3D reconstruction 631
– viscous 336, 340 – analytic 31
front end, SLAM 235 – Cartesian 31
fronto-parallel view 543, 645, 673 – epipolar 594
frustum 549 – Euclidean 31, 725
f-stop 543 – image formation 540, 553
function – steering 131
– Cauchy-Lorentz 641 Gestalt principle 495
– Dirac 410 gimbal 51, 52, 108, 117, 280
– observation 222 – lock 52, 282, 289, 316
– plenoptic 575 Global Positioning System (GPS) 150, 206, 209, 224
– point spread 543 – differential 209
– probability density (PDF) 208, 235, 759 – multi-pathing 209
– signed distance 468 – RTK 209
fundamental matrix 596, 599 – selective availability 209
– estimation 600 global shutter camera 425
GLONASS (satellite navigation system) 209
GNSS, see Global Positioning System 209
G goal seeking behavior 164
G protein-coupled receptors (GPCRs) 385 GPCRs (G protein-coupled receptors) 385
gait GPS, see Global Positioning System 209
– crawl 293 gradient descent 752, 753
– wave 293 graph 167, 495, 615, 768
Galileo (satellite navigation system) 209 – average degree 170
gamma 407 – components 170, 186, 769
– compression 407 – directed 29, 166, 269, 768
– correction 407 – edge 166, 768
– decoding 407, 422, 433, 437 – embedded 166, 768
– decompression 407 – fully connected 170
– encoding 407, 433 – node 768
– sRGB 407, 434 – non-embedded 167
ganglion layer 386 – pose 29
gantry robot 254 – search
Gaussian – A* 174, 769
– distribution 762 – admissible heuristic 175
– function 450, 759 – breadth-first (BFS) 170
– kernel 442, 443, 444, 471, 516, 520, 523 – closed set 170
 809 F–I
General Index

– cost to come 171 – corner strength 519

– cost to go 171, 175 – cumulative 237
– expansion 171 – equalization 433
– explored set 170 – normalization 433
– frontier set 170 – of image 430, 433, 437, 484, 590, 591
– heuristic distance 174 – peak finding 431, 484
– open set 170 hit and miss transform 466
– uniform-cost (UCS) 172 HOG descriptor 591
– theory 168 holonomic constraint 78, 79
– topological 167 homogeneous
– undirected 166, 768 – conic 736, 737
– vertex 166, 768 – coordinates 38, 62, 72, 726, 734
– vertex degree 169 – line, 2D 735, 737
Graphical Processing Unit (GPU) 491, 622, vii – plane 737
grassfire planner 181 – point 735, 736
Grassmann laws 391 – quadric 737
gravity – transformation 735
– acceleration 110, 113, 148, 337, 349 – transformation, 2D 36, 37
– compensation 151, 345, 362 – transformation, 3D 61, 62, 608, 612
– load 339, 345, 348, 350, 351, 357 homography 578, 604, 605, 606, 607, 609, 610, 644, 645, 647, 648
– Newtonian mechanics, in 97 – estimation 605
gray value 418, 431–433 – Euclidean 608, 645
great circle 104 – matrix 606
ground effect 147 – projective 608, 645
group 29, 605, 717, 720, 740 homothety 738
– Abelian 29 Hough transform 511, 688
– identity element 741 hovercraft 152–154
– inverse 741 Huber loss function 754
– Lie 741 hue 392, 397
– operator 93, 741 human eye 378, 383
– orthogonal 33, 720 – accommodation 541
– SE.2/ 37 – cone cell 383, 384, 385
– SE.3/ 62 – spectral response 384
– SO.2/ 33 – dynamic range 426
– SO.3/ 46 – field of view 385
– special orthogonal 720 – fovea 385
gyroscope 52, 108, 118, 131, 210 – opponent color model 386
– fibre-optic (FOG) 108 – photopic response 386
– ring-laser (RLG) 108 – retina 384
– strapdown 108 – rod cell 383, 384, 385, 426
– triaxial 109, 118 – scotopic response 386
– sensitivity 397
– theory of vision 553
H – trichromatic color model 385
Hall effect 116 – tristimulus 409
– sensor 114 humanoid robot 5, 6
Hamming distance 458 hybrid trajectory 100
hard-iron distortion 117 hydrodynamic friction 147
Harris point feature 517, 591 hyperbola 736
heading hyperboloid 565, 580, 737
– angle 114, 130, 135, 150 hysteresis threshold 450
– control 135, 150
– rate (see yaw rate) 131
helicoidal
I
– interpolation 77 IBVS, see visual servo 673
– motion 64 ICP (iterated closest point) 243, 245, 247, 650
helicopter 154 ICR (Instantaneous Center of Rotation) 142
Hessian 745, 746, 752 ideal
– approximate 747, 753 – line 550, 735
– determinant 518 – point 735
– matrix 518, 619 illuminance 388, 402
– tensor 747, 751 illumination, infrared 638
heuristic distance 174 image
higher-pair joint 255 – anaglyph 636
hippocampus 167 – binary 433
histogram – blur 425, 543
810 General Index

– border extrapolation 446 – thinning 185

– boundary detection 465 – thresholding 433, 437, 440, 450, 482
– Canny edge detector 450 – triple points 185, 466
– color 420 – warping 472, 558, 571, 634, 645, 648
– compression 420, 421, 516 – window 441, 446, 454, 456, 458
– connected components 492 Image-Based Visual Servo, see visual servo 673
– coordinates importance sampling 237
– canonical 542 impulse
– normalized, image 542 – noise 459
– retinal 542, 558 – response 441
– coordinates, retinal 552 IMU (inertial measurement unit) 53, 117
– cropping 469 – strapdown 117
– curvature 516, 518 incandescence 378
– data type inclination angle 114, 115
– float 432 inertia 339, 498
– uint8 422 – effective 339, 340, 341
– decimation 470 – link 337
– disparity 622, 623, 627 – load 340
– distance transform 177 – matrix 352, 363
– edge detector 450 – moment of 498
– end points 466 – moments of 96
– epipolar-aligned 634 – motor 339
– file format 420, 421 – product of 96, 498, 733
– from – products of 352
– camera 424 – reduction 339
– code 429 – rotational 108, 336
– file 418 – tensor 95, 96, 149, 266, 349, 641
– file sequence 424 inertial
– maps 428 – force 97
– space 428 – measurement unit (IMU) 53, 117, 711
– video 426 – navigation system (INS) 107, 117, 149, 150
– webcam 427 – parameters 338, 349
– gradient 446, 448, 449, 450, 510, 514, 516, 588, 591, 692 – reference frame 95, 97, 107
– histogram 430 inflation
– Jacobian 674, 676, 684 – obstacles 181
– line feature 510 – ratio 181
– matching 526 information matrix 232, 760
– moments 497 infrared
– monochromatic 420 – camera 412, 638
– mosaicing 646 – near (NIR) 412
– noise – pattern projector 628, 638
– impulse 459 – radiation 378, 380, 381
– reduction 449, 516 – short-wavelength (SWIR) 412
– removal 464 inner product 59, 70, 718
– salt and pepper 459 innovation 119, 220, 223, 228, 231, 764
– shot 425 INS (inertial navigation system) 107
– thermal 425 instance segmentation 482
– peak finding 457 Instantaneous Center of Rotation (ICR) 131, 142
– plane 541, 545, 735 integral
– point feature see point feature – action 345
– processing 16, 178, 184, 418, 712 – control 348
– pyramid 471 – dynamics 356
– rectification 634 – windup 345
– region Intel RealSense camera 628, 638
– feature 492 intensity
– of interest 469 – gradient 459
– resizing 470 – luminous 388
– retrieval 526 – profile 447
– segmentation 482 – surface 444, 451, 454
– similarity 454, 458, 459, 515 – wedge 407
– skeletonization 185, 466 interaction matrix, see Jacobian, image 674
– smoothing 442, 449 interest point (see point feature) 514
– sphere 567, 703 International Telecommunication Union (ITU) 393
– stabilization 649 interpolation
– stitching 646 – helicoidal 77
– subsampling 470 – linear 104
 811 I–K
General Index

– orientation 103 – higher pair 255

– pixel value 474 – lower pair 255
– scalar 285 – modeling 340
– SE.3/ 105 – prismatic 255, 259, 260
– unit quaternion 104, 105 – revolute 255, 259
inter-reflection 406 – space 285, 291
interval 466 – force 324
intromission theory 378 – generalized torque 324, 363
invariance – velocity 308, 309, 327
– brightness 524 – structure 259
– geometric 739 – transmission 334, 339, 367
– rotation 504, 516, 591 – velocity, see joint space velocity 308
– scale 504 Joseph form 764
inverse JPEG file format 421
– dynamics 348
– kinematics 278
– measurement function 224
K
– sensor model 224 Kalman filter 121, 212, 214, 221, 223, 240, 763
ISO setting 425 – extended (EKF) 119, 212, 214, 229, 765
isotropic – gain 220, 228, 764
– velocity 317 – innovation 764
– wrench 325 – iterated extended 248, 767
isotropic velocity 328 – Joseph form 764
iterated – motion model 762
– closest point (ICP) 642 – prediction 213, 763
– extended Kalman filter (IEKF) 248 – process
iterated closest point (ICP) 642, 650 – model 762
– noise 762
– sensor
J – model 762
Jacobian 309, 745 – noise 763
– analytical 311, 312, 362 – unscented 248, 767
– damped inverse 320 – update 220, 764
– derivative 361 kd-tree 593, 640
– end-effector coordinate frame 311 kernel
– geometric 310 – box 442, 445
– image 674, 676 – circular 444, 488
– ellipse feature 690 – convolution 441, 448, 454, 460
– line feature 689 – correlation 441
– photometric feature 692 – cylindrical 444
– point feature 674, 676, 680, 681, 691, 692 – density function 246
– polar coordinates 701 – Gaussian 442, 452, 471, 516, 520, 523
– spherical camera 703 – derivative of 449
– insertion 225, 227 – difference of (DoG) 453
– manipulability 317 – Laplacian 452, 520
– manipulator 308, 310, 348, 705 – of Gaussian (LoG) 452, 523
– matrix 215, 232, 289, 308–310, 747 – Marr-Hildreth 452
– numerical 747 – Mexican hat 452
– overactuated robot 322 – separable 445
– pseudoinverse 321, 322 – smoothing 442
– singularity 316, 320 – Sobel 448, 450, 476
– symbolic 747 – top-hat 444
– transpose 326 key point (see point feature) 514
– underactuated robot 321 keyframe 620
jello effect 425 keystone distortion 643, 644
jerk 98 kidnapped robot 239
JFIF file format 407 Kinect RGBD sensor 638, 639
joint kinematic
– actuator 335 – chain 255
– angles 259 – configuration 282
– configuration 259 kinematics
– control, independent 334 – differential 308
– coordinate offset 296 – forward 255, 300, 308
– coordinates 259 – inverse 278, 325
– dynamics 341 – closed-form 278, 281
– elasticity 367 – numerical 280, 283
812 General Index

– manipulator 308 lenslet array 575, 576

– velocity 308 Levenberg-Marquardt algorithm 327, 753
Klein quadric 726, 736 lidar 230, 241
k-means clustering 486, 488, 490, 504, 527, 529 – map building 244
Königsberg bridge problem 168 – noise 244
– odometry 242
– remission 242
L Lie
L a b color space 399, 408 – algebra 36, 42, 58, 64, 74, 75, 740, 741
Lambertian reflection 405, 406, 566 – group 35, 71, 740
landmark 207, 240, 245, 615 – generator 74
– ambiguity 223 light
– artificial 223 – absorption 404
– association 207 – monochromatic 378, 389, 391
– identity 223 – solar spectrum 381
Laplacian – structured 637, 638
– kernel 452, 520 – visible 378
– of Gaussian (LoG) 452, 453, 521, 523 light-field camera 574
laser rangefinder 241 line
lateral motion 131, 132, 143 – ideal 550
latus rectum 569 – in 2D 725
law –   form 510
– Asimov’s 4 – direction 725, 735
– Beer’s 381, 405 – distance to point 736
– center of gravity (color) 393 – equation of a point 735
– Grassman’s 393 – feature 510
– Moore’s vii – intersection point 735
– Newton’s – joining two points 725, 735
– first 97 – normal 725, 735
– second 97, 111, 147, 348, 371 – polar form 725
– Stefan-Boltzman 379 – in 3D 726
– Wien displacement 379 – direction 726
least squares problem – distance between 727
– linear 555, 734 – distance to point 727
– non-linear 232, 618, 753 – fronto-parallel 543
left generalized inverse 750 – intersection of planes 728
left-right consistency check 593, 627 – intersection point 727
lens 540 – joining two points 726
– aberration – moment 726
– chromatic 551 – normal 726
– spherical 551 – skew-symmetric matrix 726
– anamorphic 427 – transformation 728
– aperture 543 – of no motion 131
– cardinal points 555 line feature 510
– compound 541, 555 linearization 248, 359, 360, 364, 674, 688, 745, 766
– diopter 541 link 261
– distortion 551, 582 – inertia 341
– barrel 552 – inertial parameters 349, 362
– geometric 551 – shape 265
– parameters 556 localization 206, 226, 244, 649
– pincushion 552 – CML (concurrent mapping and localization) 227
– radial 552, 561 – error 208
– tangential 552 – lidar-based 244
– entrance pupil 555 – Monte-Carlo, sequential 235
– equation 541 – problem 208, 210
– fisheye 562, 567, 569 – SLAM (simultaneous localization and mapping) 227
– f-number 543 LoG kernel 452, 523
– focal length 541 logarithm
– focal point 541, 574 – matrix 35, 41, 57, 63
– objective 540 – principal 35, 57
– simple 540 longitude 703
– stop 426 – problem 208, 249
– telecentric 581 longitudinal motion 131
– thin 540 look up table 435
– wide-angle 561 loop closure 230
– zoom 548 LORAN 209
 813 K–M
General Index

lower-pair joint 255 – graph-based 168

lumen 386 – grid-based 167, 168
luminance 388, 391, 392, 394, 407, 425 – landmark-based 218
luminous – localization, using 218
– flux 388 – polygon-based 196
– intensity 388 mapping 226
LUT (see lookup table) 435 – concurrent mapping and localization (CML) 227
L u v color space 399 – exponential 73
LWIR 412 – lidar-based 244
– simultaneous localization and mapping (SLAM) 227
Marr-Hildreth operator 452
M mass
magnetic – center of 337
– compass 116 – matrix 352
– declination 114 matching
– dip angle 114 – 3D-2D 650
– distortion – 3D-3D 650
– hard-iron 117 mathematical
– soft-iron 117 – graph 166
– flux – morphology 184, 460
– density 114, 115, 117 – closing 463, 488
– line 114 – dilation 181, 462
– inclination 114 – erosion 462
– north 114 – hit and miss 466
– pole 114 – interval 466
magnetometer 114, 118 – opening 463
– triaxial 114, 118 – structuring element 460
Mahalanobis distance 223, 722, 760 matrix
maneuver 129, 152, 153, 154 – addition 718
Manhattan distance 177, 178, 718 – adjoint 91, 96, 300, 328, 720, 728, 744
manifold 726, 736, 740, 741, 742 – adjugate 720, 737
manipulability 289, 317, 319, 328, 680 – anti-symmetric 36, 58, 720
– dynamic 355, 356 – augmented skew-symmetric, 2D 42
– Jacobian 327 – augmented skew-symmetric, 3D 64, 72, 74
manipulator 254 – block 719
– arm 260 – camera 544, 547, 548, 556, 559, 581, 599, 620
– dexterity 289 – column space 723
– dynamics 334 – condition number 316, 680, 682, 724
– gantry 254, 260 – covariance 210, 211, 213, 216, 219, 222, 224, 226, 237, 240, 241,
– high-speed 367 760
– Jacobian 310, 324, 348 – damped inverse 320, 753
– joint, see joint 254 – decomposition
– kinematics 308 – Cholesky 722
– manipulability 289, 319 – eigen 723
– maximum payload 354 – QR 722
– number of joints 260, 274 – RQ 559
– number of links 260, 274 – singular value 445, 724, 750
– overactuated 79 – spectral 723
– Panda 285 – square root 722
– parallel link 254 – determinant 70, 316, 320, 722, 723
– planar 278 – diagonalization 723
– polar-coordinate 259 – eigenvalue 721
– pose 244 – eigenvector 721
– PUMA 560 261, 297, 341 – element-wise multiplication 718
– redundant 284, 303, 322, 323 – essential 598, 599, 610, 616, 650
– SCARA 254, 260, 283 – exponential 35, 41, 57, 58, 63, 71, 72, 74
– serial-link 255 – feature sensitivity 674
– singularity 282, 288 – generalized inverse 320, 723
– Stanform arm 260 – generator 74, 741, 743
– trajectory 240 – Hadamard product 718
– underactuated 259 – Hessian 518, 619, 746
– visual servo 705 – homogeneous transformation 37, 71
– wrist singularity 288 – identity 92, 720
– zero joint angle configuration 274, 296 – information 232, 760
manufacturing robot 3, 6 – interaction 674
map 223, 240 – inverse 720
814 General Index

– Jacobian 232, 308, 309, 674, 747 mobile robot 4, 707

– kernel 724 – aerial 146
– linear transformation 721 – car-like 130
– logarithm 35 – control 133
– moment 641 – differentially-steered 140
– Moore-Penrose pseudoinverse 320–322, 680, 681, 691, 723, 750 – localization and mapping 206
– multiplication 718 – navigation 160
– negative-definite 722, 747 – omnidirectional 143
– normal 721 – SLAM 206
– normalization 70 model
– null space 323, 597, 678, 724 – bicycle 130, 138, 143
– orthogonal 33, 46, 720 – central camera 549
– orthonormal 720 – kinematic 138, 147, 194
– positive-definite 722, 747, 755 – motion 189, 196
– positive-semi-definite 722 – quadrotor 147
– product 718 – rigid-body dynamics 147
– quadratic form 722 – unicycle 138, 141
– rank 723 model-based control 358
– rotation 34, 48, 49, 57, 60, 70, 71, 312, 751 modified Denavit-Hartenberg 298
– 2D 33 modular arithmetic 436
– 3D 46 moment
– derivative 88 – central 497, 733
– reading 48 – matrix 516, 641
– row space 723 – normalized 504
– Schur complement 758 – of inertia 498
– similar 723 – of Plücker line 726
– skew-symmetric 57, 88, 92, 720, 742, 743 – of twist 42
– 2D 36, 42 moments 497
– 3D 57, 58, 59, 71 – from perimeter 506
– 4D 580 – Hu’s 504
– sparse 758 – invariant 504
– square root 722, 731 – of inertia 96, 733
– symmetric 720 – principal, of inertia 498
– trace 721 momentum, angular 95, 108
– transpose 719 monochromatic
– visibility 615 – image 420
MAV (Micro Air Vehicle) 146 – light 378, 389, 391
maximally stable extremal regions (MSER) 494 Monte-Carlo
maximum likelihood estimator 754 – estimation 235, 246
mecanum wheel 143 – localization 235
mechanical pole 341 Moore-Penrose pseudoinverse 723
median filter 459 Moore’s law vii
MEMS (micro-electro-mechanical system) 108 Moravec interest operator 515
metamer 389 morphology (see mathematical morphology) 460
Mexican hat kernel 452 mosaicing 646
micro air vehicle (MAV) 146 motion
micro-electro-mechanical system (MEMS) 108 –  24
microlens array 575 – algebra 28
minimization, see optimization 752 – axis 101
minimum – blur 425, 543
– jerk trajectory 99 – Cartesian 105, 285
– norm solution 285, 289, 322 – control, resolved-rate 313, 315
Minkowski – detection 439
– addition 462 – helicoidal 64
– subtraction 462 – joint-space 285, 291
mirror – lateral 131, 143
– catadioptric camera 564 – longitudinal 131
– concave 561 – model
– conical 566 – bicycle 130, 189
– elliptical 565 – unicycle 140
– equiangular 565 – multi-axis 101
– hyberbolic 565 – null 25, 28
– parabolic 565 – omnidirectional 143, 165, 177, 189
– spherical 565, 566 – perceptibility 680
missing parts problem 626 – planning (see also path planning) 161
mixed pixel problem 458, 630 – rigid-body 94, 740
 815 M–O
General Index

– rotational 72, 95 nonlocal-maxima suppression 450, 451, 457, 460, 513, 517, 519, 772
– screw 65 nonparametric transform 458, 630
– segment 102 normal matrix 721
– sickness 112 normalization
– straight-line 287 – SE.3/ matrix 71
– task-space 314 – histogram 433, 475
– translational 43, 65, 72, 95 – homogeneous transformation 670, 681
motor 340 – rotation matrix 70
– limit 344 – unit dual quaternion 67
– power 344 – unit quaternion 71
– speed 100, 344 normalized
– torque 344 – cross correlation (see NCC) 455
– servo 334 – image coordinates 542, 598, 607, 675
– stepper 334 – moment 504
– torque 335, 339 north
– torque constant 335, 344, 346 – magnetic 114
MSER (maximally stable extremal regions) 494 – true 114
– descriptor 591 north-east-down (NED) 107
multi-pathing 209 notation 14, xxi
multirotor, see quadrotor 146 null space 724, 750
nullity 724
number
N – dual 67
nautical – hypercomplex 59
– angles 50 numerical optimization 280
– chronometer 208
– mile 206
navigation 155, 160
O
– chart 207 object detection
– dead reckoning 206 – deep learning 508
– inertial 88, 93, 107, 117, 150 – segmentation 492
– landmark 207 objective lens 540
– landmark-based 206, 218 obstacle inflation 181
– map-based 161, 166 occlusion 490
– marine 226 occupancy grid 164, 167, 177, 178, 244, 245, 429
– principles 206 OCR (see optical character recognition) 525
– radio 107, 209 octree 640
– reactive 160, 161 odometer 210
– spacecraft 52, 108 odometry
NCC 455 – lidar 242
near infrared (NIR) 412 – visual 210, 649
NED (north-east-down) 107 – visual (VO) 649
nested control loop 334 – wheel 210, 211, 230
Newton omnidirectional
– -Euler method 348, 370 – camera 549, 574
– first law 97 – motion 130, 143, 165, 177, 189
– method 752 – wheel 144
– -Raphson method 752 OmniSTAR 209
– second law 95, 97, 111, 147, 348, 371 Open3D 633
Newtonian telescope 561 OpenCV 13, 477
NIR 412 operational space 78
noise – control 366, 367
– Gaussian 212, 216, 223, 555, 561 – dynamics 360, 361
– image 425 operator
– impulse 459 – ı 59
– odometry 211, 213 –  134
– pixel 449, 465 – 25, 27
– process 762 – ˚ 24, 27
– sensor 220, 235, 762 –  27, 59
– thermal 425 – Canny edge 452
nomenclature xxi – closing 463
noncentral imaging 566 – convolution 441
nonholonomic 129 – correlation 441
– constraint 132, 143, 154, 652 – derivative of Gaussian 451
– system 154 – difference of Gaussian 523
nonintegrable constraint 154 – dilation 462
816 General Index

– edge 452 parallel-link robot 254

– erosion 462 particle filter 236, 241
– gradient 449 PASCAL VOC 491
– group 741 passive rotation 68
– Harris 591 path
– interest point 515 – admissible 177
– Laplacian 451 – curvature 189, 195
– Marr-Hildreth 452 – feasible 188
– monadic 482 – following 136, 188
– Morovec 515 – planning 136
– opening 463 – complete 176
– Python, overloaded 76, 422, 434, 436 – complexity 177
– Sobel edge 537 – curvature polynomial 195
– spatial 460 – D* 182
– spatial displacement 94 – distance transform 177, 199
opponent color – Dubins 189
– space 399 – global 201
– theory 386 – graph-based 168
opsins 385 – grid-based 177
optical – lattice 191
– axis 55, 540, 545 – local 201
– character recognition (OCR) 525 – map-based 166
– flow 649, 676, 684, 685, 698, 703, 704 – optimal 176, 177
– derotation 685, 711 – phase 184
– equation 676, 698 – PRM 185
– field 677 – query phase 184
optimization – rapidly exploring random tree (RRT) 196
– algorithm 327 – Reeds-Shepp 190
– bundle adjustment 615 – roadmap 184
– Gauss-Newton 753 path planning 161
– gradient descent 752 payload 334
– Levenberg-Marquardt (LM) 327 – effect of 354
– Newton-Raphson 752 PBVS, see visual servo 669
– Newton’s method 752 peak finding
– nonlinear 752 – 1D 484, 770
– least squares 753 – 2D 772
– sparse least squares 754 – refinement 624, 771, 772
– numerical 280 pencil of lines 600
– pose graph 232, 233, 235, 246, 247 perceptibility of motion 680
ORB, descriptor 591 perceptually uniform color space 399
orientation 46 perimeter
– camera 547, 559 – blob 505
– derivative 88 – circularity 506
– error 118, 119 – length 506
– estimation 109, 113, 119 – moments from 506
– in 3D 45 – shape 506
– interpolation 103 perspective
– rate of change 151 – camera 540, 564, 569, 573, 575
– vector 54 – correction 643
– vehicle 130, 139 – distortion 458, 542, 582, 588, 643
orthogonal – foreshortening 475, 543, 643
– group 720 – image 541, 561, 566
– matrix 720, 724 – projection 542, 544, 547, 550, 572, 582, 588, 674, 675
– special group in 2D 33 – weak 582
– special group, 3D 46 Perspective-n-Point (PnP) problem 561
orthonormal matrix (see orthogonal matrix) 720 photogrammetry 584, 653
orthophoto 648 photometric units 386
Otsu threshold 484 photometry 412
outer product 445, 642, 718 photopic response 386
overactuated manipulator 322 photopsin 384, 385
photoreceptor 11, 385
photosite 387, 425, 426, 546
P – array 387, 425, 546, 575, 576
panoramic camera 549 phototaxis 162
parabola 736 piano mover’s problem 196
paraboloid 565, 580, 737, 772 picket fence effect 625
 817 O–P
General Index

pincushion distortion 552 – ACF 535

pinhole camera 540, 543 – BRIEF 535
pin-hole camera 11 – BRISK 535, 591
pitch – CenSurE 591
– angle 50 – HOG 535, 591
– of twist 64 – learned 535
pixel 418 – LIFT 535
– assorted, array 387 – ORB 535, 591
– classification 482 – VLAD 535
– color value 421 – detector 514, 520, 534, 588, 589
– data type 422 – AGAST 591
– gray value 418 – FAST 535, 591
– noise 449 – Harris 517, 535, 589, 591
– value, distribution 430 – Morovec 515
planar – Noble 517
– homography 605 – Plessey 517
– transformation 739 – scale space 520, 523
Planck – Shi-Tomasi 516, 591
– constant 379 – SIFT 523, 527, 535, 589, 591
– radiation formula 379 – SURF 535
Planckian source 379 – orientation 591
plane 689, 728, 737 – strength 515, 517, 519
– point-normal form 728 – support region 523
– principal 559 pole
planner, see path planning 161 – magnetic 115
plenoptic – mechanical 341
– camera 574 – rotational 42
– function 575 polynomial
Plücker – ellipse 730
– coordinates 65, 76, 301, 579 – quintic 98
– line, see line in 3D 580, 725 pose 24
PnP (Perspective-n-Point) 561 – abstact  24
point 480 – concrete
– 2D 32 – as a 2D twist 43
– 3D 44 – as a 3D twist 66
– cloud 247, 613, 639, 642 – as a unit dual quaternion 67
– colored 614, 633, 639 – as a unit quaternion 61
– normal estimation 640 – as a vector-quaternion pair 66
– organized 640 – as an SO.2/ matrix 34, 37
– outlier removal 614 – as an SO.3/ matrix 48, 63
– plane fitting 640 – end effector 254, 308, 326
– registration 641, 642 – estimation 112, 560, 577, 583
– subsample 640 – graph 29, 67, 83, 230, 233–235, 256
– conjugate 594, 596, 598, 599, 606, 610 – in 2D 36
– coordinate 73 – in 3D 61
– coordinate vector 27, 37 – interpolation 105
– corresponding 600, 604, 605, 607, 609, 622, 627, 634 – rate of change 88, 89
– depth 676 – trajectory 98
– epipolar 604 Position-Based Visual Servoing (PBVS) 669
– equation of line, 2D 735 positive-definite matrix 722
– Euclidean 40, 736 posterior probability 214
– focal 574 posterization 435
– homogeneous 40 power
– ideal 736 – distribution, spectral (SPD) 414
– image plane 541 – motor limit 344
– landmark 614, 617 primal sketch 456
– line intersection in 2D 735 primary
– principal 541, 612 – CIE 1931 389, 392, 399
– scale-space feature 523 – color 390
– spread function 543 principal
– task space 78 – curvature 516
– vanishing 543 – moment 498
– world 541, 543, 544, 546, 548, 553, 556, 588 – plane 559
point feature 514, 515, 591, 602, 609, 610, 634 – point 545, 552, 553, 562, 566, 570, 572, 612, 649
– correspondence 589 principle
– descriptor – equivalence 97
818 General Index

– Gestalt 495 – conjugate 59

prior probability 214 – dual 67
prismatic joint 255, 259 – dual unit 67
Probabilistic Roadmap Method (PRM), see path planning 185 – Hamilton product 59
probability 210, 214, 236 – identity 60
– conditional 214 – inner product 59
– density 217, 235, 759, 760 – JPL 61, 84
– density function (PDF) 208 – matrix form 59
– Gaussian 217, 223, 759 – multiplication 59
– posterior 214 – pure 59, 89, 93
– prior 214 – unit 617
problem – unit, see unit quaternion 59
– correspondence 637 quintic polynomial 98
– missing part 626
– mixed pixel 630
process noise 211, 763
R
Procrustes transformation 738 radial distortion 552
product radiation
– of exponentials (PoE) 299, 300 – absorption 381
– of inertia 498 – electromagnetic 378
projection – infrared 378, 380, 386
– catadioptric 564 – Planck formula 379
– fisheye 563 – transittance 381
– matrix 544 – ultraviolet 378
– orthographic 581 radio navigation 107, 209
– parallel 581 radiometric units 386
– perspective 540 radius, turning 191
– re- 614 random
– stereographic 569 – dot pattern 639
– weak perspective 582 – number 236, 601, 762
projective – generator 187
– homography 578, 608 – seed 187, 486, 488, 513, 529, 530, 601, 609, 643
– plane 597 – sampling 185, 236
– reconstruction 620 – variable 759, 763, 765, 766
– space 38 range
– transformation 542, 737, 738 – measurement 241
– warping 634, 645, 648 – shadow 242
proof mass 111 rank
proper acceleration 112 – filter 459
proprioception 8 – of matrix 723
pseudo – transform 459, 590, 630
– force 97 RANSAC (Random Sampling and Consensus) algorithm 601, 607,
– inverse 723, 750 609, 610, 641
– random number 187 Rao-Blackwellized SLAM 240
PUMA 560 robot 261, 297, 341 Rapidly-Exploring Random Tree (RRT) 196
pure pursuit 136 rate of change see derivative
purple boundary 392 ratio
putative match 592 – ambiguity 625
pyramidal decomposition 471 – aspect 427, 480, 498
Python 13 – test 458, 593
PyTorch 491 raw image file 387
raxel 575, 612
recognition, of characters 525
Q reconstruction
quadrants 56, 135, 279 – affine 620
quadratic – projective 620
– form 722 rectification 634
– surface, see quadric 580 recursive Newton-Euler 348
quadric 579, 580, 737 redundant manipulator 284, 322
quadrotor 79, 146, 147, 152, 710 reflectance 382, 402, 404
– control 150 reflection
– dynamics 149 – dichromatic 406, 413
– model 147 – diffuse 405
– visual servo 709 – Fresnel 406
quantum efficiency 425 – Lambertian 405, 406, 566
quaternion 59 – specular 244, 406, 490, 566
 819 P–S
General Index

reflectivity 242, 244, 382 Rodrigues’

region – equation 75
– feature – rotation formula 56, 59
– descriptorMSER 591 – vector 57
region feature ROI (Region of Interest) 469
– area 496, 497 rolling
– aspect ratio 498 – constraint 154
– bounding box 496 – shutter camera 425
– centroid 497 roll-pitch-yaw angles 50, 52, 312
– child 506 – rate of change 151, 311
– contour 505 – singularity 51
– equivalent ellipse 498 – XYZ order 51
– hierarchy 502 – YXZ order 547
– inertia tensor 498 – ZYX order 50
– invariance 499 root finding 751
– moments 497 Rossum’s Universal Robots (RUR) 6
– MSER see maximally stable extremal regions (MSER) rotation 71
– orientation 499 – active 68
– parent 502 – angle 58
– perimeter 505 – axis 42, 55, 58, 88
– radii 498 – distance 69
– shape 506 – Euler vector 55
region of interest (ROI) 469 – extrinsic 68
relative pose 24, 27, 29 – incremental 91
remission 242 – intrinsic 68
replanning 182 – matrix
reprojection 614, 615 – in 2D 33
– error 618 – in 3D 46eigenvalues 56
resampling 237, 240 – normalization 70
resectioning 207 – passive 68
ResNet-50 network 491 – shortest distance 104
resolved-rate motion control 313, 705 rotational pole 42
retina 384, 385 rotor disk 147
– ganglion layer 386 rotorcraft, see quadrotor 146
retinal (molecule) 385 row space 723
retinal image coordinates 542, 552, 690 RRT (Rapidly exploring Random Tree) 196
retinex theory 402, 413 rule
revolute joint 255, 256, 258 – anti-cyclic rotation 52
RGBD camera 638 – cyclic rotation 52
rhodopsin 385 – right-hand 44
right-hand rule 32, 44, 718 – rotation about a vector 46
rigid body
– chain 255
– displacement 73
S
– dynamics 348 saccule 112
– motion 24 SAD 455, 459
– transformation 24 salient point (see point feature) 514
– tree 269 salt and pepper noise 459
ring-laser gyroscope (RLG) 108 SaM (Structure and Motion Estimation) 615
RLG (ring-laser gyroscope) 108 sampling
RNG (see also random number generator) 187 – artifacts 471
roadmap path planning 184 – importance 237
robot – Random Sampling and Consensus (RANSAC) 601
– aerial 146 – Shannon-Nyquist theorem 471
– definition 8, 162, 166 – spatial 471
– ethics 11 satellite navigation system 209
– field 4 saturation 392, 397
– humanoid 5 scalar field 746
– manipulator see manipulator scale
– manufacturing 4, 6 – characteristic 522
– mobile 126, 129, see mobile robot – space 449, 471, 520, 591
– service 4 – spatial 449
– surgical 8 SCARA robot 254, 260, 283
– telerobot 7 Schur complement 758
robotic vision 9 scotopic vision 386
rod cell 383 screw 64, 73
820 General Index

– axis 64, 73, 74 similarity transformation 213, 723

– motion 64, 65 simultaneous localization and mapping (SLAM) 227
– pitch 74 singular
– theory 73 – configuration 316
SE.2/ matrix 37 – value 680, 724
se.2/ matrix 42 – value decomposition (SVD) 724, 750, 751
SE.3/ matrix 62 – vector 724
se.3/ matrix 63, 64, 74, 75 singularity 52
SEA (series-elastic actuator) 368 – Euler angles 50, 53
segmentation – Jacobian 316, 320
– binary 433 – motion through 288
– color image 486 – representational 312
– graphcuts 495 – roll-pitch-yaw angles 51
– grayscale image 482 – wrist 282, 288
– MSER 494 skeletonization 185
– semantic 491 skew-symmetric matrix 88, 580, 720, 736, 742, 743
– thresholding 433 skid steering 143
selective availability 209 SLAM 227
semantic – back end 235
– label 527 – EKF 226
– segmentation 491 – Fast 240
semi-global stereo matching 629, 655 – front end 235
sensor – pose graph 230, 235
– acceleration 110, 117 – Rao-Blackwellized 240
– angular velocity 108, 109 – visual (VSLAM) 615
– bias 118, 120 slerp 104
– calibration 118 smooth
– CCD 425 – function 98
– CMOS 425 – start 688
– drift 118 smoothness constraint 631, 655
– exteroceptive 8 SO.2/ matrix 33
– fusion 117, 119, 222 so.2/ matrix 36
– gyroscopic 109 SO.3/ matrix 46
– Hall effect 114 so.3/ matrix 58
– magnetic field 114 Sobel kernel 448
– noise 118, 119 soft-iron distortion 117
– observation 219 solar spectrum 381
– proprioceptive 8 solid angle 388, 549
– range and bearing 219 SOS (standard output sensitivity) 425
– scale factor 118 space
serial-link manipulator see manipulator – affine 738
series-elastic actuator (SEA) 368 – configuration 77, 78, 152, 285
service robots 4 – Euclidean 78, 738
servo-mechanism 335 – linear 717
SfM (Structure from Motion) 615 – operational 78
shadow 410 – projective 38
– removal 410 – resectioning 583
shape – task 78, 285
– circularity 506 – vector 717
– descriptor 504 sparse
– equivalent ellipse 498 – matrix 758
– fitting 534 – stereo 610
– from moments 503 spatial
– from perimeter 505 – acceleration 355
– perimeter 507 – aliasing 471, 625
shared control 7 – displacement 94, 326
shift invariance 442 – sampling rate 471
Shi-Tomasi detector 516, 591 – velocity 89, 309, 311, 362, 674, 678, 706
short-wavelength infrared (SWIR) 412 special
shutter – Euclidean group in 2D 37
– glasses 636 – Euclidean group in 3D 62
– global 425 – orthogonal group in 2D 46
– rolling 425 – orthogonal group, 3D 33
SIFT feature detector 523, 527, 591 speckle
sigma-point filter 767 – filter 631
signed distance function 468 – projector 638
 821 S–T
General Index

spectral – and Motion (SaM) 712

– decomposition 723 – and Motion Estimation (SaM) 615
– locus 392, 393, 395, 397 – from Motion (SfM) 615
– power distribution (SPD) 414 structure tensor 516
spectral response structured light 637
– camera 411 structuring element 460
– human eye 379, 384, 386, 388–390, 409, 410 subpixel interpolation 590, 624, 773
spectrum subsumption architecture 164
– absorption 381, 405 sum of
– D65 white 401, 408 – absolute differences (see SAD) 454
– illumination 402 – squared differences (see SSD) 454
– infrared 386 support region 523, 524, 591
– luminance 382, 387, 392, 409 surface
– reflection 382 – normal 640
– solar 381 – quadratic 580, 737
– visible 381 – sphere 568, 569
specular reflection 244, 406, 490, 566 SVD (see singular value decomposition) 724
speculum, metal 566 Swedish wheel 143
sphere 580, 737 SWIR 412
spherical symbolic computation 232, 278, 308
– aberration 551 symmetric matrix 516, 720
– linear interpolation 104 system
– projection 567 – configuration 78
– wrist 260, 261, 267, 280, 281 – navigation, inertial (INS) 107, 117
spring – nonholonomic 154
– SEA 368 – nonlinear 765
– torsional 368 – reference, attitude and heading (AHRS) 118
– virtual 364 – Type 0 342, 344
SSD 455, 515, 526 – Type 1 345, 346
standard output sensitivity (SOS) 425 – Type 2 346
Stanford robot arm 260 – underactuated 79, 152
steering – vestibular 109, 112
– Ackermann 131
– differential 143
– ratio 131
T
– skid 143 Tait-Bryan angles 50
Stefan-Boltzman law 379 tangent space 741
steradian 379, 549 tangential distortion 552
stereo task space 78, 259, 285, 315, 317, 669
– anaglyph 636 – acceleration 362
– baseline 620, 626, 636, 653 – Coriolis matrix 362
– camera 621, 631, 634 – dimension 310, 320
– dense 620 – dynamics 360
– display 636 – friction 362
– failure mode 623 – gravity 362
– glasses – inertia matrix 362
– anaglyph 47 – pose 362
– matching 620, 635 – velocity 309, 362
– pair 620, 622, 634, 636 taxis 162
– perception, human 636 Taylor series 745
– sparse 610, 631, 649, 654 TCP (see also Tool Centre Point) 267
– speckle filter 631 telecentric lens 581
– system 637 telerobot 7
– triangulation 612, 631, 650 temperature
– vision 610, 628, 630, 637, 638, 653 – color 401, 411
stereographic projection 569 – Sun 381
stereopsis 614 – tungsten lamp 381
stiction 336 template matching 622
stop word 529 tensor
straight-line motion 287 – inertia 95, 96
strapdown – PyTorch 491
– gyroscope 108 – structure 516, 520
– IMU 117 Tesseract 525
– sensor configuration 108 tetrachromat 384
Stribeck friction 336 texture
Structure – lack of 629
822 General Index

– mapping 572 – homogeneous, 2D 36

– projector 638 – homogeneous, 3D 61
theorem – linear 721
– Brockett’s 154, 712 – normalization 70
– Chasles’ 73 – planar 739
– Euler’s rotation 48 – Procrustes 738
– Shannon-Nyquist sampling 471 – projective 738
theory – similarity 213, 723, 738
– extromission 378 transmission, light 405
– intromission 378 trapezoidal trajectory 100, 346
– Lie group 35, 740 traversability 166
– opponent color 386 triangle inequality 70, 96, 723
– retinex 402, 413 triangulation 207, 588, 610, 614, 650
– screw 73 triaxial
– trichromatic 385 – accelerometer 112
thin lens 540 – gyroscope 109
thinning 184, 185, 466 – magnetometer 114
thresholding 433, 440, 486 trichromat 384
– adaptive 484 trichromatic theory 385
– hysteresis 450 Tri-level hypothesis 456
– Otsu’s method 484 triple point 185, 466
tie point 647 tristimulus 422
time tristimulus values 388, 390, 392, 394, 396, 397, 399–401, 404, 407,
– of flight sensor 638 408, 411
– series data xxvi true north 114, 116
– varying pose 88, 98 Tukey biweight function 754
tone matching 648 tumbling motion 95, 109
tool center point 267 turning radius 131, 189, 191
top-hat kernel 444 twist 42, 69, 73, 299, 579, 743
topological – axis 64, 74
– graph 167 – moment 42, 74
– skeleton 184 – nonunit 65
torque – pitch 64, 74
– disturbance 334 – pole 42
– feedforward control 345, 358 – transforming 743
– gravity 337, 350 – unit 42, 64, 74
– maximum 344 – rotation, 2D 42
– motor 335 – rotation, 3D 64
– ripple 364 – translation, 2D 43
trace of matrix 721 – translation, 3D 65
traded control 7 – vector 42, 64
train 154 – velocity 328
trajectory 98, 102, 104, 241, 285, 334, 349
– Cartesian 121, 287
– hybrid 100
U
– image plane 688 UAV (uncrewed aerial vehicle) 146
– joint-space 285–287 ultrasonic rangefinder 241
– lane-changing 133 ultraviolet radiation 378, 381
– multi-axis 101 uncertainty 208, 218, 219, 221
– multi-segment 102 uncrewed aerial vehicle 146
– piecewise-linear 102 underactuated
– polynomial 98 – manipulator 321
– trapezoidal 100 – system 79, 152, 153, 154
transconductance 335 underwater imaging 406
transform 62 unicycle motion model 140
– census 458, 630 unified imaging model 569, 698
– distance 177, 467, 468 Unified Robot Description Format (URDF) 270
– nonparametric 458, 630 unit
– post-multiplication 67 – dual quaternion 67, 69
– pre-multiplication 67 – quaternion 59, 69
– rank 459, 630 – computational efficiency 60
transformation – derivative 89
– affine 473, 738 – double mapping 55, 617
– conformal 543 – interpolation 104
– Euclidean 738 – to SO.3/ matrix 60
– geometric 737, 739 – vector 61
 823 T–X
General Index

– vector part 57, 617 visibility matrix 615

– twist 42, 64, 74 visual
units – odometry (VO) 649
– candela 388 – servo 668, 698
– lumen 386 – camera retreat 686, 698
– lux 388 – ellipse feature 690
– nautical mile 206 – end-point closed-loop 668
– nit 388 – end-point open-loop 668
– photometric 386 – eye in hand 668
– radiometric 386 – holonomic robot 707
– steradian 379 – hybrid 698
Universal Transverse Mercator (UTM) 168 – image based (IBVS) 673
Unscented Kalman Filter (UKF) 248 – line feature 688
URDF (see also Unified Robot Description Format) 270 – nonholonomic robot 707
UTM, see also Universal Transverse Mercator 168 – photometric feature 692
utricle 112 – polar coordinates 701
– position based (PBVS) 669
– quadrotor 709
V – robot manipulator 705
vanishing point 543, 550 – spherical camera 703
Vaucanson’s duck 2 – XY/Z-partitioned 698
vector 717 – SLAM (VSLAM) 615
– addition 717 – vocabulary 529
– approach 54 – word 529
– cross product 718, 735 VO (visual odometry) 649
– dot product 718 von Mises distribution 211
– Euler 57, 742 Voronoi
– field 747 – cell 186
– inner product 718 – diagram 185, 186, 466
– normal 54 – roadmap 185
– tessellation 186
– orientation 54
voxel grid 640
– outer product 718
VSLAM (visual SLAM) 248, 615
– Rodrigues’ 57
– rotation 42
– scalar multiplication 717 W
– space 717
WAAS (Wide Area Augmentation System) 209
vehicle
wave gait 293
– aerial 129, 146, 154
wavefront planner 181
– Braitenberg 162
waypoint 102, 212
– car-like 130, 141
web camera (webcam) 427
– configuration 130, 189
wheel
– coordinate system 131 – base 131
– differentially-steered 130, 140 – encoder 242
– heading 130, 139, 708 – mecanum 143
– omnidirectional 143 – separation 142
– underwater 129, 154 – slip 218
– wheeled 129 white 401
velocity – D65 399, 401
– angular 88, 92, 95, 108, 109 – balance 403, 404
– control 342, 346 – equal-energy 399, 401
– ellipsoid 317 – point 397
– end-effector 308, 309, 310 Wide Area Augmentation System (WAAS) 209
– joint 308, 309, 310 Wien’s displacement law 379
– kinematics 308 world reference frame 27
– rotational 88, 91 wrench 96, 324, 326
– spatial 89, 90, 678 – base 355
– transformation 90, 96 – ellipsoid 325
– translational 88, 91 – end-effector 325, 360
– twist 91, 328 – transformation 96, 324
verged cameras 600 wrist
versor 59 – singularity 282
vertex 235, 768 – spherical 280
vertical relief 581, 647
vestibular system 109, 112, 678
via point 102
X
vignetting 425 XY/Z-partitioned IBVS 698
824 General Index

XYZ YXZ roll-pitch-yaw angles 547

– color space 395, 408, 409
– roll-pitch-yaw angles 51, 287, 312
– tristimulus value 400 Z
zero crossing detector 452
Y ZNCC 455, 456, 526, 622
– similarity measure 625
yaw
zoom lens 548
– angle 50, 130
– rate 131, 222 ZSAD 455
YCB CR color space 399 ZSSD 455
Yoshikawa’s manipulability measure 319 ZYX roll-pitch-yaw angles 50
YUV color space 399, 408 ZYZ Euler angles 49, 260