Bundle adjustment

Simple bundle adjustment class

class machinevisiontoolbox.BundleAdjust.BundleAdjust(camera)[source]

property nviews

Number of camera views

Returns:: Number of camera views
Return type:: int
Seealso:: add_view

property nlandmarks

Number of landmarks

Returns:: Number of landmarks
Return type:: int
Seealso:: add_landmark

property nstates

Length of state vector

Returns:: Length of the state vector
Return type:: int

This includes fixed views and landmarks whose state will not be updated in the opimization.

Seealso:: nvarstate

property nvarstates

Length of variable state vector

Returns:: Length of the variable state vector
Return type:: int

This is the length of the subset of the state vector that excludes fixed views and landmarks. It only includes cameras and landmarks whose state will be updated in the opimization.

Seealso:: nstates

add_view(pose, fixed=False, color='black')[source]

Add camera view to bundle adjustment problem

Parameters:

pose (SE3, array_like(7)) – camera pose
fixed (bool, optional) – the camera is fixed, defaults to False

Returns:

new camera viewpoint

Return type:

ViewPoint

Creates a camera node and adds it to the bundle adjustment problem.

The camera pose can be SE3 or a vector (1x7) comprising translation and unit quaternion in vector form.

If the camera is fixed (anchored) it will not be adjusted in the optimization process.

Note

Adds a ViewPoint object as a node in the underlying scene graph.

Seealso:: add_landmark add_projection

add_landmark(P, fixed=False)[source]

Add 3D landmark point to bundle adjustment problem

Parameters:

P (array_like(3)) – 3D world point, aka landmark
fixed (bool, optional) – the landmark is fixed, defaults to False

Returns:

new landmark

Return type:

Landmark instance

Create a landmark node and add it to the bundle adjustment problem.

If the landmark is fixed (anchored) it will not be adjusted in the optimization process.

Note

Adds a Landmark object as a node in the underlying scene graph.

Seealso:: add_view add_projection

add_projection(viewpoint, landmark, uv)[source]

Add camera observation to bundle adjustment problem

Parameters:

view (ViewPoint) – camera viewpoint
landmark (Landmark) – landmark point
uv (array_like(2)) – image plane coordinate

Add an observation by viewpoint of a landmark to the bundle adjustment problem.

Note

Adds a Observation object as an edge in the underlying scene graph.

Seealso:: add_view add_landmark

classmethod load_SBA(cameraFile, pointFile, calibFile, imagesize=None)[source]

Load bundle adjustment data files

Parameters:

cameraFile (str) – name of file with camera view data
pointFile (str) – name of file with landmark data
calibFile (str) – name of file with camera intrinsic data
imagesize (array_like(2)) – image plane dimensions in pixels, if not given infer it from principal point data in calibFile

Provides access to bundle adjustment problems from data files as distributed with the SBA package. Details of the file format are given in the source code comments.

Example:

To solve the 7-point bundle adjustment problem distributed with SBA 1.6:

>>> ba = Bundle.load_SBA('7cams.txt', '7pts.txt', 'calib.txt')
>>> X = ba.optimize()

Reference:

Sparse Bundle Adjustment package by Manolis Lourakis, http://users.ics.forth.gr/~lourakis/sba

Seealso:

add_view add_landmark add_projection

optimize(x=None, animate=False, lmbda=0.1, lmbdamin=1e-08, dxmin=0.0001, tol=0.5, iterations=1000, verbose=False)[source]

Perform the bundle adjustment

Parameters:

x – state vector, defaults to the state vector in the instance
animate (bool, optional) – graphically animate the updates, defaults to False
lmbda (float, optional) – initial damping term, defaults to 0.1
lmbdamin (float, optional) – minimum value of lmbda, defaults to 1e-8
dxmin (float, optional) – terminate optimization if state update norm falls below this threshold, defaults to 1e-4
tol (float, optional) – terminate optimization if error total reprojection error falls below this threshold, defaults to 0.5 pixels
iterations (int, optional) – maximum number of iterations, defaults to 1000
verbose (bool, optional) – show Levenberg-Marquadt status, defaults to False

Returns:

optimized state vector

Return type:

ndarray(N)

Performs a Levenberg-Marquadt style optimization of the bundle adjustment problem which repeatedly calls solve. Adjusts camera poses and landmark positions in order to minimize the total reprojection error.

Reference:

Robotics, Vision & Control for Python, Section 14.3.2, P. Corke, Springer 2023.

Seealso:

nstates solve build_linear_system

solve(x, lmbda=0.0)[source]

Solve for state update

Parameters:

x (ndarray(N)) – state vector
lmbda (float, optional) – damping term, defaults to 0.0

Returns:

\(\delta \vec{X}\), update to the variable state vector

Return type:

ndarray(M)

Determines the state update \(\delta \vec{x}\) by creating and solving the linear equation

\[\mat{H} \delta \vec{x} = \vec{b}\]

where \(\mat{H}\) is the Hessian and \(\mat{b}\) is the the projection error.

Note

The damping term lmbda is added to the diagonal of the Hessian to prevent problems when the Hessian is nearly singular.
If the problem includes fixed cameras or landmarks then \(\mbox{len}(\delta \vec{x}) < \mbox{len}(\vec{x})\) since fixed elements are omitted from the variable state vector used for the optimization.

Reference:

Robotics, Vision & Control for Python, Section 14.3.2, F.2.4, P. Corke, Springer 2023.

Seealso:

build_linear_system

build_linear_system(x)[source]

Build the linear system

Parameters:: x (ndarray(N)) – state vector
Returns:: Hessian \(\mat{H}(\vec{x})\) and projection error \(\vec{b}\)
Return type:: sparse_array(N,N), sparse_ndarray(N,1), float

Build the block structured Hessian matrix based on current bundle adjustment state and the Jacobians.

Reference:

Robotics, Vision & Control for Python, Section 14.3.2, F.2.4, P. Corke, Springer 2023.

Seealso:

spy derivatives

spyH(x, block=False)[source]

Display sparsity of Hessian

Parameters:: x (ndarray(N)) – state vector

Use Matplotlib to display the zero and non-zero elements of the Hessian.

Seealso:: build_linear_system

getstate()[source]

Get the state vector

Returns:: state vector
Return type:: ndarray(N)

Build the state vector by concatenating the pose of all cameras and then the position of all landmarks. That information is provided at problem initialization by calls to add_view and add_landmark.

Seealso:: setstate nstates add_view add_landmark

setstate(x)[source]

Update camera and landmark state

Parameters:: x (ndarray(N)) – new state vector

Copy new state data into the nodes of the bundle adjustment graph. Those nodes corresponding to fixed cameras or landmarks are unchanged.

Seealso:: updatestate getstate

updatestate(x, dx)[source]

Update the state vector

Parameters:

x (ndarray(N)) – state vector
dx (ndarray(M)) – variable state update vector

Returns:

updated state vector

Return type:

ndarray(N)

The elements of the update to the variable state are inserted into the state vector. Those elements corresponding to fixed cameras or landmarks are unchanged.

Seealso:: setstate nstates

errors(x=None)[source]

Total reprojection error

Parameters:: x (ndarray(N), optional) – state vector, defaults to state vector in instance
Returns:: total residual
Return type:: float

Compute the total reprojection error, of all projected landmarks on all camera viewpoints. Is ideally zero.

Seealso:: getresidual

getresidual(x=None)[source]

Get error residuals

Parameters:: X (ndarray(N), optional) – state vector, defaults to state vector in instance
Returns:: residuals \(\mat{R}\) for each observation
Return type:: ndarray(V,L)

Returns a 2D array \(\mat{R}\) whose elements \(r_{ij}\) represent the Euclidean reprojection error for camera \(i\) observing landmark \(j\).

Seealso:: errors

property graph

Get the scene graph

Returns:: scene graph
Return type:: PGraph

The scene graph has nodes representing camera viewpoints, of type ViewPoint, and nodes representing landmarks, of type Landmark. An edge, of type Observation, exists between a landmark and the viewpoint that observed, and the edge has the associated image plane projection.

plot(camera={}, block=None, ax=None, **kwargs)[source]

Plot the scene graph

Parameters:

camera (dict, optional) – options passed to CentralCamera.plot, defaults to {}
ax (Axes, optional) – axis to plot into, defaults to None
kwargs – options passed to PGraph.plot

Display the nodes and edges of the scene graph as an embedded graph. Overlay camera icons to indicate the camera viewpoint nodes.

Seealso:: graph

Scene graph classes

class machinevisiontoolbox.BundleAdjust.ViewPoint(x, fixed=False, color=None)[source]

property pose

Get pose of camera

Returns:: pose as an SE(3)
Return type:: SE3

adjacent()

Adjacent vertices

v.adjacent() is a list of vertices adjacent to this vertex.

Note:: For a directed graph the neighbours are those on edges leaving this vertex
Seealso:: neighbours neighbors

connect(other, **kwargs)

Connect two vertices with an edge

Parameters:

dest (Vertex subclass) – The vertex to connect to
edge (Edge subclass, optional) – Use this as the edge object, otherwise a new Edge object is created from the vertices being connected, and the cost and edge parameters, defaults to None
cost (float, optional) – the cost to traverse this edge, defaults to None
data (Any, optional) – reference to arbitrary data associated with the edge, defaults to None

Raises:

TypeError – vertex types are different subclasses

Returns:

the edge connecting the vertices

Return type:

Edge

v1.connect(v2) connects vertex v1 to vertex v2.

Note

If the vertices subclass UVertex the edge is undirected, and if they subclass DVertex the edge is directed.
Vertices must both be of the same Vertex subclass

Seealso:: Edge

property degree

Degree of vertex

Returns:: degree of the vertex
Return type:: int

Returns the number of edges connected to the vertex.

Note

For a DGraph only outgoing edges are considered.

Seealso:: edges

distance(coord)

Distance from vertex to point

Parameters:: coord (ndarray(n) or Vertex) – coordinates of the point
Returns:: distance
Return type:: float

Distance is computed according to the graph’s metric.

Seealso:: metric

edges()

All outgoing edges of vertex

Returns:: List of all edges leaving this vertex
Return type:: list of Edge

Note

For a directed graph the edges are those leaving this vertex
For a non-directed graph the edges are those leaving or entering
this vertex

edgeto(dest)

Get edge connecting vertex to specific neighbour

Parameters:: dest (Vertex subclass) – a neigbouring vertex
Raises:: ValueError – dest is not a neighbour
Returns:: the edge from this vertex to dest
Return type:: Edge

Note

For a directed graph dest must be at the arrow end of the edge

incidences()

Neighbours and edges of a vertex

v.incidences() is a generator that returns a list of incidences, tuples of (vertex, edge) for all neighbours of the vertex v.

Note

For a directed graph the edges are those leaving this vertex

property index

Index into the state vector (base method)

Returns:: the index of the start of this object’s state in the state vector
Return type:: int

property index2

Index into the variable state vector (base method)

Returns:: the index of the start of this object’s state in the variable state vector
Return type:: int

property isfixed

Value is fixed (base method)

Returns:: Quantity is fixed
Return type:: bool

This viewpoint or landmark will not be adjusted during optimization.

isneighbour(vertex)

Test if vertex is a neigbour

Parameters:: vertex (Vertex subclass) – vertex reference
Returns:: true if a neighbour
Return type:: bool

For a directed graph this is true only if the edge is from self to vertex.

neighbors()

Neighbors of a vertex

v.neighbors() is a list of neighbors of this vertex.

Note

For a directed graph the neighbors are those on edges leaving this vertex
US English synonym for adjacent

Seealso:: adjacent neighbours

neighbours()

Neighbours of a vertex

v.neighbours() is a list of neighbours of this vertex.

Note

For a directed graph the neighbours are those on edges leaving this vertex
British English synonym for adjacent

Seealso:: adjacent neighbours

property x

The x-coordinate of an embedded vertex

Returns:: The x-coordinate
Return type:: float

property y

The y-coordinate of an embedded vertex

Returns:: The y-coordinate
Return type:: float

property z

The z-coordinate of an embedded vertex

Returns:: The z-coordinate
Return type:: float

class machinevisiontoolbox.BundleAdjust.Landmark(P, fixed=False)[source]

property P

Get landmark position

Returns:: landmark position in 3D
Return type:: ndarray(3)

adjacent()

Adjacent vertices

v.adjacent() is a list of vertices adjacent to this vertex.

Note:: For a directed graph the neighbours are those on edges leaving this vertex
Seealso:: neighbours neighbors

connect(other, **kwargs)

Connect two vertices with an edge

Parameters:

dest (Vertex subclass) – The vertex to connect to
edge (Edge subclass, optional) – Use this as the edge object, otherwise a new Edge object is created from the vertices being connected, and the cost and edge parameters, defaults to None
cost (float, optional) – the cost to traverse this edge, defaults to None
data (Any, optional) – reference to arbitrary data associated with the edge, defaults to None

Raises:

TypeError – vertex types are different subclasses

Returns:

the edge connecting the vertices

Return type:

Edge

v1.connect(v2) connects vertex v1 to vertex v2.

Note

If the vertices subclass UVertex the edge is undirected, and if they subclass DVertex the edge is directed.
Vertices must both be of the same Vertex subclass

Seealso:: Edge

property degree

Degree of vertex

Returns:: degree of the vertex
Return type:: int

Returns the number of edges connected to the vertex.

Note

For a DGraph only outgoing edges are considered.

Seealso:: edges

distance(coord)

Distance from vertex to point

Parameters:: coord (ndarray(n) or Vertex) – coordinates of the point
Returns:: distance
Return type:: float

Distance is computed according to the graph’s metric.

Seealso:: metric

edges()

All outgoing edges of vertex

Returns:: List of all edges leaving this vertex
Return type:: list of Edge

Note

For a directed graph the edges are those leaving this vertex
For a non-directed graph the edges are those leaving or entering
this vertex

edgeto(dest)

Get edge connecting vertex to specific neighbour

Parameters:: dest (Vertex subclass) – a neigbouring vertex
Raises:: ValueError – dest is not a neighbour
Returns:: the edge from this vertex to dest
Return type:: Edge

Note

For a directed graph dest must be at the arrow end of the edge

incidences()

Neighbours and edges of a vertex

v.incidences() is a generator that returns a list of incidences, tuples of (vertex, edge) for all neighbours of the vertex v.

Note

For a directed graph the edges are those leaving this vertex

property index

Index into the state vector (base method)

Returns:: the index of the start of this object’s state in the state vector
Return type:: int

property index2

Index into the variable state vector (base method)

Returns:: the index of the start of this object’s state in the variable state vector
Return type:: int

property isfixed

Value is fixed (base method)

Returns:: Quantity is fixed
Return type:: bool

This viewpoint or landmark will not be adjusted during optimization.

isneighbour(vertex)

Test if vertex is a neigbour

Parameters:: vertex (Vertex subclass) – vertex reference
Returns:: true if a neighbour
Return type:: bool

For a directed graph this is true only if the edge is from self to vertex.

neighbors()

Neighbors of a vertex

v.neighbors() is a list of neighbors of this vertex.

Note

For a directed graph the neighbors are those on edges leaving this vertex
US English synonym for adjacent

Seealso:: adjacent neighbours

neighbours()

Neighbours of a vertex

v.neighbours() is a list of neighbours of this vertex.

Note

For a directed graph the neighbours are those on edges leaving this vertex
British English synonym for adjacent

Seealso:: adjacent neighbours

property x

The x-coordinate of an embedded vertex

Returns:: The x-coordinate
Return type:: float

property y

The y-coordinate of an embedded vertex

Returns:: The y-coordinate
Return type:: float

property z

The z-coordinate of an embedded vertex

Returns:: The z-coordinate
Return type:: float

class machinevisiontoolbox.BundleAdjust.Observation(camera, landmark, uv)[source]

property p

Get image plane projection

Returns:: observed projection of landmark on image plane
Return type:: ndarray(2)

connect(v1, v2)

Add edge to the graph

Parameters:

v1 (Vertex subclass) – start of the edge
v2 (Vertex subclass) – end of the edge

The edge is added to the graph and connects vertices v1 and v2.

Note

The vertices must already be added to the graph.

next(vertex)

Return other end of an edge

Parameters:: vertex (Vertex subclass) – one vertex on the edge
Raises:: ValueError – vertex is not on the edge
Returns:: the other vertex on the edge
Return type:: Vertex subclass

e.next(v1) is the vertex at the other end of edge e, ie. the vertex that is not v1.