This namespace contains some extra tools for ublas vector and matrix. More...

Detailed Description

This namespace contains some extra tools for ublas vector and matrix.

Functions
jblas::ind_array	ia_set (const jblas::vecb &vb)
	Create array from a boolean vector.
jblas::ind_array	ia_set (const ublas::range &r)
	Create indirect array from range.
jblas::ind_array	ia_set (size_t begin, size_t end)
	Create indirect array from start and end indices.
jblas::ind_array	ia_set (const ublas::slice &s)
	Create indirect array from slice.
jblas::ind_array	ia_complement (const jblas::ind_array &ifull, const jblas::ind_array &ipartial)
	Find the elements in indirect array ifull that are not contained in ipartial.
jblas::ind_array	ia_union (const jblas::ind_array &ia1, const jblas::ind_array &ia2)
	Make the union of two indirect arrays.
jblas::ind_array	ia_concat (const jblas::ind_array &ia1, const jblas::ind_array &ia2)
	Make the concatenation of two indirect arrays.
jblas::ind_array	ia_head (const jblas::ind_array &ia, const size_t N)
	Return the N first indices of an indirect array.
jblas::ind_array	ia_head (const jblas::vecb &vb, const size_t N)
	Create indirect array from boolean vector and take the head N elements The size of the returned array is the number of requested elements.
jblas::ind_array	ia_popfront (jblas::vecb &vb, const size_t N)
	Create indirect array from boolean vector with the head N true-elements, and clear these N elements form the boolean.
jblas::ind_array	ia_pushfront (jblas::vecb &vb, const size_t N)
	Create indirect array from boolean vector with the head N false-elements, and set these N elements to the boolean.
jblas::ind_array	operator+ (const jblas::ind_array &ia, int n)
	Add an integer to all indexes of the indirect array.
jblas::ind_array	operator- (const jblas::ind_array &ia, int n)
template<class bubMatrix >
void	ixaxpy_offdiag_subprod (sym_mat &S, const ind_array &ia_invariant, const bubMatrix &J_oi, const ind_array &ia_in, const ind_array &ia_out)
template<class bubMatrix , class bubMatrixY >
void	ixaxpy_diag_subprod (sym_mat &S, const bubMatrix &J, const ind_array &ia_in, const ind_array &ia_out, const bubMatrixY Y)
template<class bubMatrix >
void	ixaxpy_diag_subprod (sym_mat &S, const bubMatrix &J, const ind_array &ia_in, const ind_array &ia_out)
template<class bubMatrixJ , class bubMatrixY >
void	ixaxpy_prod (sym_mat &X, const ind_array &ia_invariant, const bubMatrixJ &A_oi, const ind_array &ia_in, const ind_array &ia_out, const bubMatrixY Y_oo)
	Symmetric, sparse matrix product with indirect indexing.
template<class bubMatrixJ >
void	ixaxpy_prod (sym_mat &X, const ind_array &ia_invariant, const bubMatrixJ &A_oi, const ind_array &ia_in, const ind_array &ia_out)
	Symmetric, sparse matrix product with indirect indexing.
template<class V >
void	setVectorValue (V &v, const double *val_, std::size_t size_)
template<class V >
void	fillVector (V &v, const double &value=0.0)
	Fills vector with a certain value.
template<class V , typename W >
void	fillVector (V &out, const W *in)
	Fills vector with the contents of a c-array.
template<int size, typename W >
jblas::vec	createVector (const W in[size])
	Creates a vector with the contents of a c-array.
template<class T >
jblas::vec	createVector (T const &v, size_t pos, size_t size)
	general subrange from jblas::vec or c array to jblas::vec
template<int size, typename W >
jblas::sym_mat	createSymMat (const W std[size])
	Creates a vector with the contents of a c-array.
template<class T >
jblas::sym_mat	createSymMat (T const &v, size_t shift, size_t csize, size_t pos, size_t size)
	Convert a subblock of an aligned full symmetric matrix (jblas::vec or c array) to a sym_mat.
template<class T >
void	explodeSymMat (jblas::sym_mat const &m, T &v, size_t shift, size_t csize, size_t pos, size_t size)
	Copy a sym_mat to a subblock of an aligned full symmetric matrix (jblas::vec or c array)
template<class V >
bool	isZeroVector (const V &v)
template<typename T >
ublas::diagonal_matrix< T >	scalar_diag_mat (const size_t size, const T val)
	create diagonal matrix from scalar.
template<class V >
jblas::diag_mat	vec_diag_mat (const V &v)
	Create diagonal matrix from vector.
template<class V >
void	normalize (V &v)
	normalize a vector.
template<class V >
jblas::vec	normalized (V const &v)
	normalized vector.
template<class M1 , class M2 >
void	lu_inv (M1 const &m, M2 &inv)
	General matrix inversion routine.
template<class SymMat >
jblas::sym_mat	prod_JPJt (const SymMat &P, const jblas::mat &J)
	Covariance transformation using jacobians (JPJt)
template<class SymMat , class V >
double	prod_xt_P_x (const SymMat &P, const V &x)
	Normalized estimation error squared (x^TP^-1x)
template<class SymMat , class V >
double	prod_xt_iP_x (SymMat &iP, V &x)
	Normalized estimation error squared (x^TP^-1x)
template<class V , class M >
void	normalizeJac (V &v, M &J)
	Jacobian of normalize().
template<std::size_t N, class Vec1 , class Vec2 , class Mat >
void	inner_prodJac (Vec1 const &u1, Vec2 const &u2, Mat &J)
	Compute the jacobian of inner product.
template<class Vec , class Mat >
double	norm_2 (Vec const &u, Mat &J)
	Compute the jacobian of norm_2.
template<std::size_t N, class Vec , class Mat >
void	norm_2Jac (Vec const &u, Mat &J)
template<class Vec1 , class Vec2 , class VecRes >
void	crossProd (Vec1 const &v1, Vec2 const &v2, VecRes &vRes)
	Compute the cross product of v1 and v2, result is stored in vRes.
template<class Vec1 , class Vec2 >
jblas::vec3	crossProd (Vec1 const &v1, Vec2 const &v2)
	Compute the cross product of v1 and v2, the result is returned.
template<class Mat >
std::string	prettyFormat (Mat const &m_)
	Format a matrix output it to a string.
template<class Mat >
std::string	matlabFormat (Mat const &m_)
template<class M >
void	setMatrixValue (M &m, const double *val_, std::size_t size1_, std::size_t size2_)
template<class M >
double	max (const M &m_)
	Find maximum value of a matrix.
template<class V >
double	maxV (const V &v_)
	Find maximum value of a matrix.
template<class M >
double	trace (const M &m_)
	Computes the trace of a matrix.
template<class M >
double	lu_det (M const &m)
	General matrix determinant.
template<class M >
double	det (const M &m_)
template<class M >
void	inv (const M &m_, M &m_inv)
jblas::vec2	eigenValues (const jblas::mat22 &m)
template<typename T >
void	minus_vector (const ublas::matrix< T > &M, const ublas::vector< T > &v, ublas::matrix< T > &M_v)
	computes per row M minus v and store it in M_v
template<typename T >
void	minus_vector (ublas::matrix< T > &M, const ublas::vector< T > &v)
	computes per row M minus v and store it in M : move it into a matrix vector expression like ublas::prod
template<typename T >
void	plus_vector (ublas::matrix< T > &M, const ublas::vector< T > &v)
	computes per row M plus v and store it in M : move it into a matrix vector expression like ublas::prod
template<typename T >
void	delete_row (ublas::matrix< T > &M, size_t index)
	deletes a matrix M(nxm) row in a memory efficient way i.e.
template<typename T >
void	delete_column (ublas::matrix< T > &M, size_t index)
	deletes a matrix M(nxm) column in a memory efficient way i.e.
template<typename T >
void	delete_columns (ublas::matrix< T > &M, std::vector< size_t > indices, bool is_sorted=false)
	deletes a list of column indices stored in indices from M
template<typename T >
void	delete_rows (ublas::matrix< T > &M, std::vector< size_t > indices, bool is_sorted=false)
	deletes a list of row indices stored in indices from M

Function Documentation

template<class T >

jblas::sym_mat jafar::jmath::ublasExtra::createSymMat	(	T const &	v,
		size_t	shift,
		size_t	csize,
		size_t	pos,
		size_t	size
	)

Convert a subblock of an aligned full symmetric matrix (jblas::vec or c array) to a sym_mat.

Parameters:

v	the vector that contains the upper triangle aligned
shift	constant shift
csize	container size
pos	block position
size	block size

Definition at line 133 of file ublasExtra.hpp.

template<class T >

jblas::vec jafar::jmath::ublasExtra::createVector	(	T const &	v,
		size_t	pos,
		size_t	size
	)

general subrange from jblas::vec or c array to jblas::vec

Parameters:

v	the whole vector
pos	block position
size	block size

Definition at line 103 of file ublasExtra.hpp.

template<typename T >

void jafar::jmath::ublasExtra::delete_column	(	ublas::matrix< T > &	M,
		size_t	index
	)

deletes a matrix M(nxm) column in a memory efficient way i.e.

no extra memory is allocated.

Returns:: M with size nxm-1.

Definition at line 765 of file ublasExtra.hpp.

References JFR_ASSERT.

Referenced by delete_columns().

template<typename T >

void jafar::jmath::ublasExtra::delete_columns	(	ublas::matrix< T > &	M,
		std::vector< size_t >	indices,
		bool	is_sorted = `false`
	)

deletes a list of column indices stored in indices from M

Parameters:

indices,:	vector of indices considered
is_sorted,:	are the indices sorted into an acending order?

Returns:: M with size nxm-(indices.size()).

Definition at line 783 of file ublasExtra.hpp.

References delete_column(), and JFR_ASSERT.

template<typename T >

void jafar::jmath::ublasExtra::delete_row	(	ublas::matrix< T > &	M,
		size_t	index
	)

deletes a matrix M(nxm) row in a memory efficient way i.e.

no extra memory is allocated.

Returns:: M with size n-1xm.

Definition at line 747 of file ublasExtra.hpp.

References JFR_ASSERT.

Referenced by delete_rows().

template<typename T >

void jafar::jmath::ublasExtra::delete_rows	(	ublas::matrix< T > &	M,
		std::vector< size_t >	indices,
		bool	is_sorted = `false`
	)

deletes a list of row indices stored in indices from M

Parameters:

indices,:	vector of indices considered
is_sorted,:	are the indices sorted into an acending order?

Returns:: M with size nxm-(indices.size()).

Definition at line 803 of file ublasExtra.hpp.

References delete_row(), and JFR_ASSERT.

jblas::vec2 jafar::jmath::ublasExtra::eigenValues ( const jblas::mat22 & m ) [inline]

Returns:: the eigen values of a 2x2 matrix

Definition at line 684 of file ublasExtra.hpp.

template<class T >

void jafar::jmath::ublasExtra::explodeSymMat	(	jblas::sym_mat const &	m,
		T &	v,
		size_t	shift,
		size_t	csize,
		size_t	pos,
		size_t	size
	)

Copy a sym_mat to a subblock of an aligned full symmetric matrix (jblas::vec or c array)

Parameters:

m	the matrix to convert
v	the vector that will contains the upper triangle aligned (needs to be allocated to the right size)
shift	constant shift
csize	container size
pos	block position
size	block size

Definition at line 156 of file ublasExtra.hpp.

jblas::ind_array jafar::jmath::ublasExtra::ia_complement	(	const jblas::ind_array &	ifull,
		const jblas::ind_array &	ipartial
	)

Find the elements in indirect array ifull that are not contained in ipartial.

Parameters:

ifull	the full indirect array
ipartial	the partial indirect array

Returns:: the complementary indirect array

jblas::ind_array jafar::jmath::ublasExtra::ia_concat	(	const jblas::ind_array &	ia1,
		const jblas::ind_array &	ia2
	)

Make the concatenation of two indirect arrays.

Parameters:

ia1	an indirect array
ia2	an indirect array

Returns:: the simple concatenation of ia1 and ia2

Referenced by jafar::rtslam::hardware::HardwareSensorAbstract< T >::getRaws().

jblas::ind_array jafar::jmath::ublasExtra::ia_head	(	const jblas::ind_array &	ia,
		const size_t	N
	)

Return the N first indices of an indirect array.

Parameters:

ia	the indirect array
N	the number of indices to return

Returns:: the N first indices in ia

jblas::ind_array jafar::jmath::ublasExtra::ia_head	(	const jblas::vecb &	vb,
		const size_t	N
	)

Create indirect array from boolean vector and take the head N elements The size of the returned array is the number of requested elements.

It is an error if not enough elements are present in the input boolean vector.

Parameters:

vb	the boolean vector.
N	the number of elements to be returned.

Returns:: the indirect array.

jblas::ind_array jafar::jmath::ublasExtra::ia_popfront	(	jblas::vecb &	vb,
		const size_t	N
	)

Create indirect array from boolean vector with the head N true-elements, and clear these N elements form the boolean.

The size of the returned array is the number of requested elements. It is an error if not enough true elements are present in the input boolean vector.

Parameters:

vb	the boolean vector.
N	the number of elements to be returned.

Returns:: the indirect array.

jblas::ind_array jafar::jmath::ublasExtra::ia_pushfront	(	jblas::vecb &	vb,
		const size_t	N
	)

Create indirect array from boolean vector with the head N false-elements, and set these N elements to the boolean.

The size of the returned array is the number of requested elements. It is an error if not enough false elements are present in the input boolean vector.

Parameters:

vb	the boolean vector.
N	the number of elements to be returned.

Returns:: the indirect array.

jblas::ind_array jafar::jmath::ublasExtra::ia_set ( const jblas::vecb & vb )

Create array from a boolean vector.

Create array from a integer vector.

The size of the returned indirect array is the number of true entries in the boolean vector. The indices in the array are the positions of the true values in the boolean vector.

Parameters:

vb	a boolean vector

Returns:: the indirect array

The size of the returned indirect array is the number of non-zero entries in the integer vector. The indices in the array are the positions of the non-zero values in the integer vector.

Parameters:

vi	an integer vector

Returns:: the indirect array

jblas::ind_array jafar::jmath::ublasExtra::ia_set ( const ublas::range & r )

Create indirect array from range.

Parameters:

r a range.

Returns:: the indirect array.

jblas::ind_array jafar::jmath::ublasExtra::ia_set	(	size_t	begin,
		size_t	end
	)

Create indirect array from start and end indices.

The indirect array goes from begin to (end -1).

Parameters:

begin	the first index
end	the last index plus one.

Returns:: the indirect array.

jblas::ind_array jafar::jmath::ublasExtra::ia_set ( const ublas::slice & s )

Create indirect array from slice.

Parameters:

s a slice.

Returns:: the indirect array.

jblas::ind_array jafar::jmath::ublasExtra::ia_union	(	const jblas::ind_array &	ia1,
		const jblas::ind_array &	ia2
	)

Make the union of two indirect arrays.

Parameters:

ia1	an indirect array
ia2	an indirect array

Returns:: the union of ia1 and ia2, with non-repeating indices, sorted from smallest to largest.

Referenced by jafar::rtslam::LandmarkAnchoredHomogeneousPointsLine::nobs().

template<class bubMatrixJ , class bubMatrixY >

void jafar::jmath::ublasExtra::ixaxpy_prod	(	sym_mat &	X,
		const ind_array &	ia_invariant,
		const bubMatrixJ &	A_oi,
		const ind_array &	ia_in,
		const ind_array &	ia_out,
		const bubMatrixY	Y_oo
	)

Symmetric, sparse matrix product with indirect indexing.

This function performs the product X = A*X*A' + Y where:

X is a symmetric matrix having the role of a covariances matrix of a certain vector x,
A is a sparse Jacobian matrix, A=[A_oi 0 0; 0 I 0;0 0 0], which maps the elements of x indexed by indirect arrays ia_in into the elements indexed by ia_out, that is:
- x(ia_out) = A_oi * x(ia_in).
Y_oo is a symmetric matrix that is added to the ia_out covariance. We have Y = [Y_oo 0;0 0]

Due to the sparsity of the problem and to the existence of identity blocks in the Jacobian A, a significant part of the symmetric matrix X is unchanged. The indices to these unchanged entries must be given to the function via ia_invariant.

To explain the operation, one has to consider four parts in the storage state vector, x = [s, n] = [in, out, inv, n], where:

s is the used part of x.
in is the input part of s (of x) affected by A_oi.
out is the output part of s (of x) affected by A_oi.
inv is the part of s (of x) not affected by A_oi.
n is the part of x that is not used at all, and it only serves as reserved storage.

We index each part with indirect arrays, ia_x = [ia_s, ia_n] = [ia_in, ia_out, ia_inv, ia_n], where:

ia_x: the full indirect array pointing to all terms of x. x(ia_x) is the same as x(:) in Matlab.
ia_s: the part of used states in the vector. x(ia_s) = s.
ia_in: the part of input states to the Jacobian. x(ia_in) = in;
ia_out: the part of output states from the Jacobian. x(ia_out) = out;
ia_inv: the part of states not affected by the product. x(ia_inv) = inv.
ia_n: the part of non-used states (ignored, not used, just here to explain). x(ia_n) = n.

Then:

X(ia_out,ia_out) = A_oi * X(ia_in,ia_in) * A_oi' + Y
X(ia_out,ia_inv) = A_oi* X(ia_in,ia_inv)
X(ia_inv,ia_out) is not computed as the matrix X is symmetric.
X(ia_inv,ia_inv) is unchanged
X(ia_n,ia_x) and its transpose X(ia_x,ia_n) are ignored.

Parameters:

X	a storage covariances matrix
ia_invariant	the indices in x of invariant states
A_oi	a Jacobian matrix maping x(in) into x(out)=A_oix*(in)
ia_in	the indices in x that are to be affected by A_oi.
ia_out	the indices in x resulting from the A_oi mapping
Y	a symmetric matrix to be added to the output square block X(ia_out,ia_out)

Definition at line 101 of file ixaxpy.hpp.

template<class bubMatrixJ >

void jafar::jmath::ublasExtra::ixaxpy_prod	(	sym_mat &	X,
		const ind_array &	ia_invariant,
		const bubMatrixJ &	A_oi,
		const ind_array &	ia_in,
		const ind_array &	ia_out
	)

Symmetric, sparse matrix product with indirect indexing.

This function performs the product X = A*X*A' when X is a symmetric matrix and A is sparse.

See also:: See the overloaded companion for more detailed information.

Definition at line 115 of file ixaxpy.hpp.

template<class M >

double jafar::jmath::ublasExtra::lu_det ( M const & m )

General matrix determinant.

It uses lu_factorize in uBLAS.

Definition at line 589 of file ublasExtra.hpp.

References JFR_PRECOND, and JFR_TRACE_END.

template<class M1 , class M2 >

void jafar::jmath::ublasExtra::lu_inv	(	M1 const &	m,
		M2 &	inv
	)

General matrix inversion routine.

It uses lu_factorize and lu_substitute in uBLAS to invert a matrix

Definition at line 243 of file ublasExtra.hpp.

References JFR_PRECOND, and JFR_TRACE_END.

Referenced by jafar::rtslam::SensorAbsloc::process(), and prod_xt_P_x().

template<class Mat >

std::string jafar::jmath::ublasExtra::matlabFormat ( Mat const & m_ )

Format a matrix output to a string in matlab syntax

Definition at line 467 of file ublasExtra.hpp.

template<class M >

double jafar::jmath::ublasExtra::max ( const M & m_ )

Find maximum value of a matrix.

Warning:: it returns a double whatever matrix type...

Definition at line 546 of file ublasExtra.hpp.

Referenced by maxV().

template<class V >

double jafar::jmath::ublasExtra::maxV ( const V & v_ )

Find maximum value of a matrix.

Warning:: it returns a double whatever matrix type...

Definition at line 562 of file ublasExtra.hpp.

References max().

template<typename T >

void jafar::jmath::ublasExtra::minus_vector	(	const ublas::matrix< T > &	M,
		const ublas::vector< T > &	v,
		ublas::matrix< T > &	M_v
	)

computes per row M minus v and store it in M_v

Returns:: M_v : move it into a matrix vector expression like ublas::prod

Definition at line 699 of file ublasExtra.hpp.

References JFR_ASSERT.

template<class Mat >

std::string jafar::jmath::ublasExtra::prettyFormat ( Mat const & m_ )

Format a matrix output it to a string.

Each matrix row corresponds to a line.

Definition at line 452 of file ublasExtra.hpp.

template<class SymMat >

jblas::sym_mat jafar::jmath::ublasExtra::prod_JPJt	(	const SymMat &	P,
		const jblas::mat &	J
	)

Covariance transformation using jacobians (J*P*Jt)

Parameters:

P	the covariances matrix
J	the Jacobian matrix

Returns:: the transformed covariances matrix

Definition at line 274 of file ublasExtra.hpp.

References JFR_PRECOND.

Referenced by jafar::rtslam::quaternion::e2q(), jafar::rtslam::SensorAbsloc::process(), jafar::rtslam::quaternion::q2e(), and jafar::rtslam::RobotAbstract::slamPoseToRobotPose().

template<class SymMat , class V >

double jafar::jmath::ublasExtra::prod_xt_iP_x	(	SymMat &	iP,
		V &	x
	)

Normalized estimation error squared (x^T*P^-1*x)

Parameters:

iP	the inverse of the covariances matrix
x	the vector

Returns:: the NEES = x' * iP * x

Definition at line 302 of file ublasExtra.hpp.

References JFR_PRECOND.

template<class SymMat , class V >

double jafar::jmath::ublasExtra::prod_xt_P_x	(	const SymMat &	P,
		const V &	x
	)

Normalized estimation error squared (x^T*P^-1*x)

Parameters:

P	the covariances matrix
x	the vector

Returns:: the NEES = x' * P^-1 * x

Definition at line 287 of file ublasExtra.hpp.

References JFR_PRECOND, and lu_inv().

template<typename T >

ublas::diagonal_matrix<T> jafar::jmath::ublasExtra::scalar_diag_mat	(	const size_t	size,
		const T	val
	)

create diagonal matrix from scalar.

The matrix is:

M(i,i) = val <-- diagonal
M(i,j) = 0 <-- off-diagonal

Parameters:

size	the size of the (square) matrix
val	the value to set to the diagonal elements

Definition at line 192 of file ublasExtra.hpp.

template<class M >

double jafar::jmath::ublasExtra::trace ( const M & m_ )

Computes the trace of a matrix.

Warning:: it returns a double whatever matrix type...

Definition at line 575 of file ublasExtra.hpp.

References JFR_PRECOND.

template<class V >

jblas::diag_mat jafar::jmath::ublasExtra::vec_diag_mat ( const V & v )

Create diagonal matrix from vector.

The matrix is:

M(i,i) = v(i) <-- diagonal
M(i,j) = 0 <-- off-diagonal

Parameters:

v	the vector with the values to set to the diagonal

Definition at line 209 of file ublasExtra.hpp.

Detailed Description

Functions

Function Documentation