josch/3dpcp
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity
3dpcp/include/slam6d/globals.icc

1292 lines
32 KiB
Text
Raw Normal View History

initial commit
2012-09-16 12:33:11 +00:00
/**
* @file
* @brief Globally used functions
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#ifndef __GLOBALS_ICC__
#define __GLOBALS_ICC__
#ifdef _MSC_VER
#include <windows.h>
#define _USE_MATH_DEFINES
#include <math.h>
inline int gettimeofday (struct timeval* tp, void* tzp)
{
unsigned long t;
t = timeGetTime();
tp->tv_sec = t / 1000;
tp->tv_usec = t % 1000;
return 0; /* 0 indicates success. */
}
#else
#include <sys/time.h>
#endif
#include <algorithm>
using std::min;
using std::max;
#include <cmath>
#include <sstream>
using std::stringstream;
#include <fstream>
using std::ostream;
using std::istream;
#include <iostream>
using std::cout;
using std::endl;
#include <iomanip>
#include <stdexcept>
using std::runtime_error;
/**
* Set bits count
*
* @param unsigned char x
*
* @return char
*
*/
inline unsigned char _my_popcount_3(unsigned char x) {
x -= (x >> 1) & 0x55; //put count of each 2 bits into those 2 bits
x = (x & 0x33) + ((x >> 2) & 0x33); //put count of each 4 bits into those 4 bits
x = (x + (x >> 4)) & 0x0f; //put count of each 8 bits into those 8 bits
return x;
}
/**
* Converts a class T to a string of width with padding 0
*
* @param t output
* @param width length
*
* @return string of t
*
*/
template <class T>
inline std::string to_string(const T& t, int width)
{
stringstream ss;
ss << std::setfill('0') << std::setw(width) << t;
return ss.str();
}
/**
* Converts a class T to a string of width with padding 0
*
* @param t output
* @return string of t
*
*/
template <class T>
inline std::string to_string(const T& t)
{
stringstream ss;
ss << t;
return ss.str();
}
/**
* Overridden "<<" operator for sending a (4x4)-matrix to a stream
*
* @param os stream
* @param matrix 4x4 matrix sent to stream
* @return stream
*/
inline ostream& operator<<(ostream& os, const double matrix[16])
{
for (int i = 0; i < 16; os << matrix[i++] << " ");
return os;
}
/**
* Overridden ">>" operator for reading a (4x4)-matrix from a stream.<br>
* Throws a runtime error if not enough data in the stream.
*
* @param is stream
* @param matrix 4x4 matrix sent to stream
* @return stream
*/
inline istream& operator>>(istream& is, double matrix[16])
{
for (int i = 0; i < 16; i++) {
if (!is.good()) throw runtime_error("Not enough elements to read for >>(istream&, double[16])");
is >> matrix[i];
}
return is;
}
/**
* Converts an angle (given in deg) to rad
*
* @param deg integer indicating, whether the figure to be drawn to show
* the clusters should be circles (0) or rectangles(1)
*
* @return the clustered image, with the clusters marked by colored figures
*
*/
template <class T>
inline T rad(const T deg)
{
return ( (2 * M_PI * deg) / 360 );
}
/**
* Converts an angle (given in rad) to deg
*
* @param rad angle in rad
* @return angle in deg
*/
template <class T>
inline T deg(const T rad)
{
return ( (rad * 360) / (2 * M_PI) );
}
/**
* Calculates x^2
*
* @param x input scalar value
* @return squared value
*/
template <class T>
static inline T sqr(const T &x)
{
return x*x;
}
/**
* Computes the <i>squared</i> length of a 3-vector
*
* @param x input 3-vector
* @return length^2 of vector
*/
template <class T>
inline T Len2(const T *x)
{
return sqr(x[0]) + sqr(x[1]) + sqr(x[2]);
}
/**
* Computes the length of a 3-vector
*
* @param x input 3-vector
* @return length of vector
*/
template <class T>
inline T Len(const T *x)
{
return sqrt(Len2(x));
}
/**
* Computes the <i>squared</i> Eucledian distance between two points
* in 3-space
*
* @param x1 first input vector
* @param x2 decond input vecotr
* @return Eucledian distance^2 between the two locations
*/
template <class T, class F>
inline T Dist2(const T *x1, const F *x2)
{
T dx = x2[0] - x1[0];
T dy = x2[1] - x1[1];
T dz = x2[2] - x1[2];
return sqr(dx) + sqr(dy) + sqr(dz);
}
/*
* Normalization of the input 3-vector
*
* @param x input/output 3-vector
*/
template <class T>
static inline void Normalize3(T *x)
{
T norm = sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2]);
x[0] /= norm;
x[1] /= norm;
x[2] /= norm;
}
/*
* Normalization of the input 4-vector
*
* @param x input/output 4-vector
*/
template <class T>
static inline void Normalize4(T *x)
{
T norm = sqrt((x[0]*x[0] + x[1]*x[1] + x[2]*x[2] + x[3]*x[3]));
x[0] /= norm;
x[1] /= norm;
x[2] /= norm;
x[3] /= norm;
}
/**
* Sets a 4x4 matrix to identity
*
* @param M 4x4 matrix
*/
template <class T>
inline void M4identity( T *M )
{
M[0] = M[5] = M[10] = M[15] = 1.0;
M[1] = M[2] = M[3] = M[4] = M[6] = M[7] = M[8] = M[9] = M[11] = M[12] = M[13] = M[14] = 0.0;
}
/**
* Multiplies a 4x4 matrices in OpenGL
* (column-major) order
*
* @param M1 first input matrix
* @param M2 second input matrix
* @param Mout output matrix
*
*/
template <class T>
inline void MMult(const T *M1,
const T *M2,
T *Mout)
{
Mout[ 0] = M1[ 0]*M2[ 0]+M1[ 4]*M2[ 1]+M1[ 8]*M2[ 2]+M1[12]*M2[ 3];
Mout[ 1] = M1[ 1]*M2[ 0]+M1[ 5]*M2[ 1]+M1[ 9]*M2[ 2]+M1[13]*M2[ 3];
Mout[ 2] = M1[ 2]*M2[ 0]+M1[ 6]*M2[ 1]+M1[10]*M2[ 2]+M1[14]*M2[ 3];
Mout[ 3] = M1[ 3]*M2[ 0]+M1[ 7]*M2[ 1]+M1[11]*M2[ 2]+M1[15]*M2[ 3];
Mout[ 4] = M1[ 0]*M2[ 4]+M1[ 4]*M2[ 5]+M1[ 8]*M2[ 6]+M1[12]*M2[ 7];
Mout[ 5] = M1[ 1]*M2[ 4]+M1[ 5]*M2[ 5]+M1[ 9]*M2[ 6]+M1[13]*M2[ 7];
Mout[ 6] = M1[ 2]*M2[ 4]+M1[ 6]*M2[ 5]+M1[10]*M2[ 6]+M1[14]*M2[ 7];
Mout[ 7] = M1[ 3]*M2[ 4]+M1[ 7]*M2[ 5]+M1[11]*M2[ 6]+M1[15]*M2[ 7];
Mout[ 8] = M1[ 0]*M2[ 8]+M1[ 4]*M2[ 9]+M1[ 8]*M2[10]+M1[12]*M2[11];
Mout[ 9] = M1[ 1]*M2[ 8]+M1[ 5]*M2[ 9]+M1[ 9]*M2[10]+M1[13]*M2[11];
Mout[10] = M1[ 2]*M2[ 8]+M1[ 6]*M2[ 9]+M1[10]*M2[10]+M1[14]*M2[11];
Mout[11] = M1[ 3]*M2[ 8]+M1[ 7]*M2[ 9]+M1[11]*M2[10]+M1[15]*M2[11];
Mout[12] = M1[ 0]*M2[12]+M1[ 4]*M2[13]+M1[ 8]*M2[14]+M1[12]*M2[15];
Mout[13] = M1[ 1]*M2[12]+M1[ 5]*M2[13]+M1[ 9]*M2[14]+M1[13]*M2[15];
Mout[14] = M1[ 2]*M2[12]+M1[ 6]*M2[13]+M1[10]*M2[14]+M1[14]*M2[15];
Mout[15] = M1[ 3]*M2[12]+M1[ 7]*M2[13]+M1[11]*M2[14]+M1[15]*M2[15];
}
template <class T>
inline void MMult(const T *M1,
const T *M2,
float *Mout)
{
Mout[ 0] = M1[ 0]*M2[ 0]+M1[ 4]*M2[ 1]+M1[ 8]*M2[ 2]+M1[12]*M2[ 3];
Mout[ 1] = M1[ 1]*M2[ 0]+M1[ 5]*M2[ 1]+M1[ 9]*M2[ 2]+M1[13]*M2[ 3];
Mout[ 2] = M1[ 2]*M2[ 0]+M1[ 6]*M2[ 1]+M1[10]*M2[ 2]+M1[14]*M2[ 3];
Mout[ 3] = M1[ 3]*M2[ 0]+M1[ 7]*M2[ 1]+M1[11]*M2[ 2]+M1[15]*M2[ 3];
Mout[ 4] = M1[ 0]*M2[ 4]+M1[ 4]*M2[ 5]+M1[ 8]*M2[ 6]+M1[12]*M2[ 7];
Mout[ 5] = M1[ 1]*M2[ 4]+M1[ 5]*M2[ 5]+M1[ 9]*M2[ 6]+M1[13]*M2[ 7];
Mout[ 6] = M1[ 2]*M2[ 4]+M1[ 6]*M2[ 5]+M1[10]*M2[ 6]+M1[14]*M2[ 7];
Mout[ 7] = M1[ 3]*M2[ 4]+M1[ 7]*M2[ 5]+M1[11]*M2[ 6]+M1[15]*M2[ 7];
Mout[ 8] = M1[ 0]*M2[ 8]+M1[ 4]*M2[ 9]+M1[ 8]*M2[10]+M1[12]*M2[11];
Mout[ 9] = M1[ 1]*M2[ 8]+M1[ 5]*M2[ 9]+M1[ 9]*M2[10]+M1[13]*M2[11];
Mout[10] = M1[ 2]*M2[ 8]+M1[ 6]*M2[ 9]+M1[10]*M2[10]+M1[14]*M2[11];
Mout[11] = M1[ 3]*M2[ 8]+M1[ 7]*M2[ 9]+M1[11]*M2[10]+M1[15]*M2[11];
Mout[12] = M1[ 0]*M2[12]+M1[ 4]*M2[13]+M1[ 8]*M2[14]+M1[12]*M2[15];
Mout[13] = M1[ 1]*M2[12]+M1[ 5]*M2[13]+M1[ 9]*M2[14]+M1[13]*M2[15];
Mout[14] = M1[ 2]*M2[12]+M1[ 6]*M2[13]+M1[10]*M2[14]+M1[14]*M2[15];
Mout[15] = M1[ 3]*M2[12]+M1[ 7]*M2[13]+M1[11]*M2[14]+M1[15]*M2[15];
}
/**
* Transforms a vactor with a matrix (P = M * V)
* @param M the transformation matrix
* @param v the initial vector
* @param p the transformt vector
*/
template <class T>
inline void VTrans(const T *M, const T *V, T *P)
{
P[0] = M[0] * V[0] + M[4] * V[1] + M[8] * V[2] + M[12];
P[1] = M[1] * V[0] + M[5] * V[1] + M[9] * V[2] + M[13];
P[2] = M[2] * V[0] + M[6] * V[1] + M[10] * V[2] + M[14];
}
/**
* Converts an Euler angle to a 3x3 matrix
*
* @param rPosTheta vector of Euler angles
* @param alignxf 3x3 matrix corresponding to the Euler angles
*/
inline void EulerToMatrix3(const double *rPosTheta, double *alignxf)
{
double sx = sin(rPosTheta[0]);
double cx = cos(rPosTheta[0]);
double sy = sin(rPosTheta[1]);
double cy = cos(rPosTheta[1]);
double sz = sin(rPosTheta[2]);
double cz = cos(rPosTheta[2]);
alignxf[0] = cy*cz;
alignxf[1] = sx*sy*cz + cx*sz;
alignxf[2] = -cx*sy*cz + sx*sz;
alignxf[3] = -cy*sz;
alignxf[4] = -sx*sy*sz + cx*cz;
alignxf[5] = cx*sy*sz + sx*cz;
alignxf[6] = sy;
alignxf[7] = -sx*cy;
alignxf[8] = cx*cy;
}
/**
* Calculates the determinant of a 3x3 matrix
*
* @param M input 3x3 matrix
* @return determinant of input matrix
*/
template <class T>
inline double M3det( const T *M )
{
double det;
det = (double)(M[0] * ( M[4]*M[8] - M[7]*M[5] )
- M[1] * ( M[3]*M[8] - M[6]*M[5] )
+ M[2] * ( M[3]*M[7] - M[6]*M[4] ));
return ( det );
}
/**
* Inverts a 3x3 matrix
*
* @param Min input 3x3 matrix
* @param Mout output 3x3 matrix
*/
template <class T>
inline void M3inv( const T *Min, T *Mout )
{
double det = M3det( Min );
if ( fabs( det ) < 0.0005 ) {
M3identity( Mout );
return;
}
Mout[0] = (double)( Min[4]*Min[8] - Min[5]*Min[7] ) / det;
Mout[1] = (double)(-( Min[1]*Min[8] - Min[7]*Min[2] )) / det;
Mout[2] = (double)( Min[1]*Min[5] - Min[4]*Min[2] ) / det;
Mout[3] = (double)(-( Min[3]*Min[8] - Min[5]*Min[6] )) / det;
Mout[4] = (double)( Min[0]*Min[8] - Min[6]*Min[2] ) / det;
Mout[5] = (double)(-( Min[0]*Min[5] - Min[3]*Min[2] )) / det;
Mout[6] = (double) ( Min[3]*Min[7] - Min[6]*Min[4] ) / det;
Mout[7] = (double)(-( Min[0]*Min[7] - Min[6]*Min[1] )) / det;
Mout[8] = (double) ( Min[0]*Min[4] - Min[1]*Min[3] ) / det;
}
/**
* Converts a pose into a RT matrix
* @param *rPos Pointer to the position (double[3])
* @param *rPosTheta Pointer to the angles (double[3])
* @param *alignxf The calculated matrix
*/
inline void EulerToMatrix4(const double *rPos, const double *rPosTheta, double *alignxf)
{
double sx = sin(rPosTheta[0]);
double cx = cos(rPosTheta[0]);
double sy = sin(rPosTheta[1]);
double cy = cos(rPosTheta[1]);
double sz = sin(rPosTheta[2]);
double cz = cos(rPosTheta[2]);
alignxf[0] = cy*cz;
alignxf[1] = sx*sy*cz + cx*sz;
alignxf[2] = -cx*sy*cz + sx*sz;
alignxf[3] = 0.0;
alignxf[4] = -cy*sz;
alignxf[5] = -sx*sy*sz + cx*cz;
alignxf[6] = cx*sy*sz + sx*cz;
alignxf[7] = 0.0;
alignxf[8] = sy;
alignxf[9] = -sx*cy;
alignxf[10] = cx*cy;
alignxf[11] = 0.0;
alignxf[12] = rPos[0];
alignxf[13] = rPos[1];
alignxf[14] = rPos[2];
alignxf[15] = 1;
}
/**
* Converts a 4x4 matrix to Euler angles.
*
* @param alignxf input 4x4 matrix
* @param rPosTheta output 3-vector of Euler angles
* @param rPos output vector of trnaslation (position) if set
*
*/
static inline void Matrix4ToEuler(const double *alignxf, double *rPosTheta, double *rPos = 0)
{
double _trX, _trY;
if(alignxf[0] > 0.0) {
rPosTheta[1] = asin(alignxf[8]);
} else {
rPosTheta[1] = M_PI - asin(alignxf[8]);
}
// rPosTheta[1] = asin( alignxf[8]); // Calculate Y-axis angle
double C = cos( rPosTheta[1] );
if ( fabs( C ) > 0.005 ) { // Gimball lock?
_trX = alignxf[10] / C; // No, so get X-axis angle
_trY = -alignxf[9] / C;
rPosTheta[0] = atan2( _trY, _trX );
_trX = alignxf[0] / C; // Get Z-axis angle
_trY = -alignxf[4] / C;
rPosTheta[2] = atan2( _trY, _trX );
} else { // Gimball lock has occurred
rPosTheta[0] = 0.0; // Set X-axis angle to zero
_trX = alignxf[5]; //1 // And calculate Z-axis angle
_trY = alignxf[1]; //2
rPosTheta[2] = atan2( _trY, _trX );
}
rPosTheta[0] = rPosTheta[0];
rPosTheta[1] = rPosTheta[1];
rPosTheta[2] = rPosTheta[2];
if (rPos != 0) {
rPos[0] = alignxf[12];
rPos[1] = alignxf[13];
rPos[2] = alignxf[14];
}
}
/**
* Sets a 3x3 matrix to the identity matrix
*
* @param M input 3x3 matrix
*/
template <class T>
static inline void M3identity( T *M )
{
M[0] = M[4] = M[8] = 1.0;
M[1] = M[2] = M[3] = M[5] = M[6] = M[7] = 0.0;
}
/**
* Gets the current time (in ms)
*
* @return current time (in ms)
*/
static inline unsigned long GetCurrentTimeInMilliSec()
{
static unsigned long milliseconds;
#ifdef _MSC_VER
SYSTEMTIME stime;
GetSystemTime(&stime);
milliseconds = ((stime.wHour * 60 + stime.wMinute) * 60 + stime.wSecond) * 1000 + stime.wMilliseconds;
#else
static struct timeval tv;
gettimeofday(&tv, NULL);
milliseconds = tv.tv_sec * 1000 + tv.tv_usec / 1000;
#endif
return milliseconds;
}
/**
* generates random numbers in [0..rnd]
*
* @param rnd maximum number
* @return random number between 0 and rnd
*/
inline int rand(int rnd)
{
return (int) ((double)rnd * (double)std::rand() / (RAND_MAX + 1.0));
}
/**
* generates unsigned character random numbers in [0..rnd]
*
* @param rnd maximum number
* @return random number between 0 and rnd
*/
inline unsigned char randUC(unsigned char rnd)
{
return (unsigned char) ((float)rnd * std::rand() / (RAND_MAX + 1.0));
}
/**
* Computes the angle between 2 points in polar coordinates
*/
inline double polardist(double* p, double *p2) {
double stheta = sin(p[0]) * sin(p2[0]);
double myd2 = acos( stheta * cos(p[1]) * cos(p2[1])
+ stheta * sin(p[1]) * sin(p2[1])
+ cos(p[0]) * cos(p2[0]));
return myd2;
}
inline void toKartesian(double *polar, double *kart) {
kart[0] = polar[2] * cos( polar[1] ) * sin( polar[0] );
kart[1] = polar[2] * sin( polar[1] ) * sin( polar[0] );
kart[2] = polar[2] * cos( polar[0] );
}
/**
* Transforms a point in cartesian coordinates into polar
* coordinates
*/
inline void toPolar(double *n, double *polar) {
double phi, theta, rho;
rho = Len(n);
Normalize3(n);
// if(fabs(1 - fabs(n[1])) < 0.001) {
// cout << "Y " << n[0] << " " << n[1] << " " << n[2] << endl;
phi = acos(n[2]);
//if ( fabs(phi) < 0.0001) phi = 0.0001;
//if ( fabs(M_PI - phi) < 0.0001) phi = 0.0001;
double theta0;
if(fabs(phi) < 0.0001) {
theta = 0.0;
} else if(fabs(M_PI - phi) < 0.0001) {
theta = 0.0;
} else {
if(fabs(n[0]/sin(phi)) > 1.0) {
if(n[0]/sin(phi) < 0) {
theta0 = M_PI;
} else {
theta0 = 0.0;
}
} else {
theta0 = acos(n[0]/sin(phi));
}
double sintheta = n[1]/sin(phi);
double EPS = 0.0001;
if(fabs(sin(theta0) - sintheta) < EPS) {
theta = theta0;
} else if(fabs( sin( 2*M_PI - theta0 ) - sintheta ) < EPS) {
theta = 2*M_PI - theta0;
} else {
theta = 0;
cout << "Fehler" << endl;
}
}
/* } else {
theta = 0.0;
phi = 0.0;
}*/
polar[0] = phi;
polar[1] = theta;
polar[2] = rho;
}
/*
* Computes the submatrix without
* row i and column j
*
* @param Min input 4x4 matrix
* @param Mout output 3x3 matrix
* @param i row index i
* @param j column index j
*/
template <class T>
static inline void M4_submat(const T *Min, T *Mout, int i, int j ) {
int di, dj, si, sj;
// loop through 3x3 submatrix
for( di = 0; di < 3; di ++ ) {
for( dj = 0; dj < 3; dj ++ ) {
// map 3x3 element (destination) to 4x4 element (source)
si = di + ( ( di >= i ) ? 1 : 0 );
sj = dj + ( ( dj >= j ) ? 1 : 0 );
// copy element
Mout[di * 3 + dj] = Min[si * 4 + sj];
}
}
}
/*
* Computes the determinant of a 4x4 matrix
*
* @param 4x4 matrix
* @return determinant
*/
template <class T>
static inline double M4det(const T *M )
{
T det, result = 0, i = 1.0;
T Msub3[9];
int n;
for ( n = 0; n < 4; n++, i *= -1.0 ) {
M4_submat( M, Msub3, 0, n );
det = M3det( Msub3 );
result += M[n] * det * i;
}
return( result );
}
/*
* invert a 4x4 Matrix
*
* @param Min input 4x4 matrix
* @param Mout output matrix
* @return 1 if successful
*/
template <class T>
static inline int M4inv(const T *Min, T *Mout )
{
T mdet = M4det( Min );
if ( fabs( mdet ) < 0.0005 ) {
cout << "Error matrix inverting!" << endl;
M4identity( Mout );
return( 0 );
}
T mtemp[9];
int i, j, sign;
for ( i = 0; i < 4; i++ ) {
for ( j = 0; j < 4; j++ ) {
sign = 1 - ( (i +j) % 2 ) * 2;
M4_submat( Min, mtemp, i, j );
Mout[i+j*4] = ( M3det( mtemp ) * sign ) / mdet;
}
}
return( 1 );
}
/*
* transposes a 4x4 matrix
*
* @param Min input 4x4 matrix
* @param Mout output 4x4 matrix
*/
template <class T>
static inline int M4transpose(const T *Min, T *Mout )
{
Mout[0] = Min[0];
Mout[4] = Min[1];
Mout[8] = Min[2];
Mout[12] = Min[3];
Mout[1] = Min[4];
Mout[5] = Min[5];
Mout[9] = Min[6];
Mout[13] = Min[7];
Mout[2] = Min[8];
Mout[6] = Min[9];
Mout[10] = Min[10];
Mout[14] = Min[11];
Mout[3] = Min[12];
Mout[7] = Min[13];
Mout[11] = Min[14];
Mout[15] = Min[15];
return( 1 );
}
/* +++++++++-------------++++++++++++
* NAME
* choldc
* DESCRIPTION
* Cholesky Decomposition of a symmetric
* positive definite matrix
* Overwrites lower triangle of matrix
* Numerical Recipes, but has a bit of
* the fancy C++ template thing happening
* +++++++++-------------++++++++++++ */
static inline bool choldc(double A[3][3], double diag[3])
{
for (unsigned int i = 0; i < 3; i++) {
for (unsigned int j = i; j < 3; j++) {
double sum = A[i][j];
for (int k=i-1; k >= 0; k--)
sum -= A[i][k] * A[j][k];
if (i == j) {
if (sum < 1.0e-7)
return false;
diag[i] = sqrt(sum);
} else {
A[j][i] = sum / diag[i];
}
}
}
return true;
}
/* +++++++++-------------++++++++++++
* NAME
* choldc
* DESCRIPTION
* Cholesky Decomposition of a symmetric
* positive definite matrix
* Overwrites lower triangle of matrix
* Numerical Recipes, but has a bit of
* the fancy C++ template thing happening
* +++++++++-------------++++++++++++ */
static inline bool choldc(unsigned int n, double **A, double *diag)
{
for (unsigned int i = 0; i < n; i++) {
for (unsigned int j = i; j < n; j++) {
double sum = A[i][j];
for (int k=i-1; k >= 0; k--)
sum -= A[i][k] * A[j][k];
if (i == j) {
if (sum < 1.0e-7)
return false;
diag[i] = sqrt(sum);
} else {
A[j][i] = sum / diag[i];
}
}
}
return true;
}
/* +++++++++-------------++++++++++++
* NAME
* cholsl
* DESCRIPTION
* Solve Ax=B after choldc
* +++++++++-------------++++++++++++ */
static inline void cholsl(double A[3][3],
double diag[3],
double B[3],
double x[3])
{
for (int i=0; i < 3; i++) {
double sum = B[i];
for (int k=i-1; k >= 0; k--)
sum -= A[i][k] * x[k];
x[i] = sum / diag[i];
}
for (int i=2; i >= 0; i--) {
double sum = x[i];
for (int k=i+1; k < 3; k++)
sum -= A[k][i] * x[k];
x[i] = sum / diag[i];
}
}
/* +++++++++-------------++++++++++++
* NAME
* cholsl
* DESCRIPTION
* Solve Ax=B after choldc
* +++++++++-------------++++++++++++ */
static inline void cholsl(unsigned int n,
double **A,
double *diag,
double *B,
double *x)
{
for (unsigned int i=0; i < n; i++) {
double sum = B[i];
for (int k=(int)i-1; k >= 0; k--)
sum -= A[i][k] * x[k];
x[i] = sum / diag[i];
}
for (int i=(int)n-1; i >= 0; i--) {
double sum = x[i];
for (unsigned int k=i+1; k < n; k++)
sum -= A[k][i] * x[k];
x[i] = sum / diag[i];
}
}
/**
* Transforms a a quaternion and a translation vector into a 4x4
* Matrix
*
* @param quat input quaternion
* @param t input translation
* @param mat output matrix
*/
static inline void QuatToMatrix4(const double *quat, const double *t, double *mat)
{
// double q00 = quat[0]*quat[0];
double q11 = quat[1]*quat[1];
double q22 = quat[2]*quat[2];
double q33 = quat[3]*quat[3];
double q03 = quat[0]*quat[3];
double q13 = quat[1]*quat[3];
double q23 = quat[2]*quat[3];
double q02 = quat[0]*quat[2];
double q12 = quat[1]*quat[2];
double q01 = quat[0]*quat[1];
mat[0] = 1 - 2 * (q22 + q33);
mat[5] = 1 - 2 * (q11 + q33);
mat[10] = 1 - 2 * (q11 + q22);
mat[4] = 2.0*(q12-q03);
mat[1] = 2.0*(q12+q03);
mat[8] = 2.0*(q13+q02);
mat[2] = 2.0*(q13-q02);
mat[9] = 2.0*(q23-q01);
mat[6] = 2.0*(q23+q01);
mat[3] = mat[7] = mat[11] = 0.0;
if (t == 0) {
mat[12] = mat[13] = mat[14] = 0.0;
} else {
mat[12] = t[0];
mat[13] = t[1];
mat[14] = t[2];
}
mat[15] = 1.0;
}
/**
* Transforms a 4x4 Transformation Matrix into a quaternion
*
* @param mat matrix to be converted
* @param quat resulting quaternion
* @param t resulting translation
*/
static inline void Matrix4ToQuat(const double *mat, double *quat, double *t = 0)
{
double T, S, X, Y, Z, W;
T = 1 + mat[0] + mat[5] + mat[10];
if ( T > 0.00000001 ) { // to avoid large distortions!
S = sqrt(T) * 2;
X = ( mat[9] - mat[6] ) / S;
Y = ( mat[2] - mat[8] ) / S;
Z = ( mat[4] - mat[1] ) / S;
W = 0.25 * S;
} else if ( mat[0] > mat[5] && mat[0] > mat[10] ) { // Column 0:
S = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2;
X = 0.25 * S;
Y = (mat[4] + mat[1] ) / S;
Z = (mat[2] + mat[8] ) / S;
W = (mat[9] - mat[6] ) / S;
} else if ( mat[5] > mat[10] ) { // Column 1:
S = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2;
X = (mat[4] + mat[1] ) / S;
Y = 0.25 * S;
Z = (mat[9] + mat[6] ) / S;
W = (mat[2] - mat[8] ) / S;
} else { // Column 2:
S = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2;
X = (mat[2] + mat[8] ) / S;
Y = (mat[9] + mat[6] ) / S;
Z = 0.25 * S;
W = (mat[4] - mat[1] ) / S;
}
quat[0] = W;
quat[1] = -X;
quat[2] = -Y;
quat[3] = -Z;
Normalize4(quat);
if (t != 0) {
t[0] = mat[12];
t[1] = mat[13];
t[2] = mat[14];
}
}
/**
* Transforms a Quaternion to the corresponding Axis-Angle representation
*
* @param quat 4-vector of quaternion
* gets overridden by the axis/angle representation
*/
static inline void QuatToAA(double *quat){
//double x, y, z, w;
double sum = 0.0;
double cos_a, angle, x, y, z, sin_a;
for(int i = 0; i < 4; i++){
sum += quat[i]*quat[i];
}
sum = sqrt(sum);
//quaternion_normalise( |W,X,Y,Z| );
cos_a = quat[0]/sum;
angle = acos( cos_a ) * 2;
sin_a = sqrt( 1.0 - cos_a * cos_a );
if ( fabs( sin_a ) < 0.0005 ) sin_a = 1;
x = quat[1] / sin_a;
y = quat[2] / sin_a;
z = quat[3] / sin_a;
quat[0] = angle;
quat[1] = x;
quat[2] = y;
quat[3] = z;
}
/**
* Quaternion Multiplication q1 * q2 = q3
*/
static inline void QMult(const double *q1, const double *q2, double *q3) {
q3[0] = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3];
q3[1] = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2];
q3[2] = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1];
q3[3] = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0];
}
/**
* Quaternion SLERP
* http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/
*/
static inline void slerp(const double *qa, const double *qb, const double t, double *qm) {
// Calculate angle between them.
double cosHalfTheta = qa[0] * qb[0] + qa[1] * qb[1] + qa[2] * qb[2] + qa[3] * qb[3];
// if qa=qb or qa=-qb then theta = 0 and we can return qa
if (fabs(cosHalfTheta) >= 1.0) {
qm[0] = qa[0];
qm[1] = qa[1];
qm[2] = qa[2];
qm[3] = qa[3];
return;
}
// Calculate temporary values.
double halfTheta = acos(cosHalfTheta);
double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
// if theta = 180 degrees then result is not fully defined
// we could rotate around any axis normal to qa or qb
if (fabs(sinHalfTheta) < 0.001){
qm[0] = (qa[0] * 0.5 + qb[0] * 0.5);
qm[1] = (qa[1] * 0.5 + qb[1] * 0.5);
qm[2] = (qa[2] * 0.5 + qb[2] * 0.5);
qm[3] = (qa[3] * 0.5 + qb[3] * 0.5);
Normalize4(qm);
return;
}
double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
double ratioB = sin(t * halfTheta) / sinHalfTheta;
//calculate Quaternion.
qm[0] = (qa[0] * ratioA + qb[0] * ratioB);
qm[1] = (qa[1] * ratioA + qb[1] * ratioB);
qm[2] = (qa[2] * ratioA + qb[2] * ratioB);
qm[3] = (qa[3] * ratioA + qb[3] * ratioB);
Normalize4(qm);
}
/* taken from ROOT (CERN)
* as well in:
* Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning
* James J. Kuffner
*
* Distance between two rotations in Quaternion form
* Note: The rotation group is isomorphic to a 3-sphere
* with diametrically opposite points identified.
* The (rotation group-invariant) is the smaller
* of the two possible angles between the images of
* the two rotations on that sphere. Thus the distance
* is never greater than pi/2.
*/
inline double quat_dist(double quat1[4], double quat2[4]) {
double chordLength = std::fabs(quat1[0]*quat2[0] + quat1[1]*quat2[1] + quat1[2]*quat2[2] + quat1[3]*quat2[3]);
if (chordLength > 1) chordLength = 1; // in case roundoff fouls us up
return acos(chordLength) / M_PI * 180.0;
}
/**
* Converts a Rotation given by Axis-Angle and a Translation into a
* 4x4 Transformation matrix
*
* @param aa axis and angle aa[0] is the angle
* @param trans vector containing the translation
* @param matrix matrix to be computed
*/
inline void AAToMatrix(double *aa, double *trans, double *matrix ){
double rcos = cos(aa[0]);
double rsin = sin(aa[0]);
double u = aa[1];
double v = aa[2];
double w = aa[3];
matrix[0] = rcos + u*u*(1-rcos);
matrix[1] = w * rsin + v*u*(1-rcos);
matrix[2] = -v * rsin + w*u*(1-rcos);
matrix[3] = 0.0;
matrix[4] = -w * rsin + u*v*(1-rcos);
matrix[5] = rcos + v*v*(1-rcos);
matrix[6] = u * rsin + w*v*(1-rcos);
matrix[7] = 0.0;
matrix[8] = v * rsin + u*w*(1-rcos);
matrix[9] = -u * rsin + v*w*(1-rcos);
matrix[10] = rcos + w*w*(1-rcos);
matrix[11] = 0.0;
matrix[12] = trans[0];
matrix[13] = trans[1];
matrix[14] = trans[2];
matrix[15] = 1.0;
}
/**
* Factors matrix A into lower and upper triangular matrices
* (L and U respectively) in solving the linear equation Ax=b.
*
* @param A (input/output) Matrix(1:n, 1:n) In input, matrix to be
* factored. On output, overwritten with lower and
* upper triangular factors.
*
* @param indx (output) Vector(1:n) Pivot vector. Describes how
* the rows of A were reordered to increase
* numerical stability.
*
* @return return int(0 if successful, 1 otherwise)
*/
inline int LU_factor( double A[4][4], int indx[4])
{
int M = 4;
int N = 4;
int i=0,j=0,k=0;
int jp=0;
double t;
int minMN = 4;
for (j = 0; j < minMN; j++)
{
// find pivot in column j and test for singularity.
jp = j;
t = fabs(A[j][j]);
for (i = j+1; i < M; i++)
if ( fabs(A[i][j]) > t)
{
jp = i;
t = fabs(A[i][j]);
}
indx[j] = jp;
// jp now has the index of maximum element
// of column j, below the diagonal
if ( A[jp][j] == 0 )
return 1; // factorization failed because of zero pivot
if (jp != j) // swap rows j and jp
for (k = 0; k < N; k++)
{
t = A[j][k];
A[j][k] = A[jp][k];
A[jp][k] =t;
}
if (j < M) // compute elements j+1:M of jth column
{
// note A(j,j), was A(jp,p) previously which was
// guarranteed not to be zero (Label #1)
//
double recp = 1.0 / A[j][j];
for (k = j+1; k < M; k++)
A[k][j] *= recp;
}
if (j < minMN)
{
// rank-1 update to trailing submatrix: E = E - x*y;
//
// E is the region A(j+1:M, j+1:N)
// x is the column vector A(j+1:M,j)
// y is row vector A(j,j+1:N)
int ii,jj;
for (ii = j+1; ii < M; ii++)
for (jj = j+1; jj < N; jj++)
A[ii][jj] -= A[ii][j]*A[j][jj];
}
}
return 0;
}
/**
* Solves a linear system via LU after LU factor
*
* @param A 4x4 matrix
* @param indx indices
* @param b 4 vectort
*
* @return 0
*
*/
inline int LU_solve(const double A[4][4], const int indx[4], double b[4])
{
int i,ii=0,ip,j;
int n = 4;
double sum = 0.0;
for (i = 0; i < n; i++)
{
ip=indx[i];
sum=b[ip];
b[ip]=b[i];
if (ii)
for (j = ii;j <= i-1; j++)
sum -= A[i][j]*b[j];
else if (sum) ii=i;
b[i]=sum;
}
for (i = n-1; i >= 0; i--)
{
sum=b[i];
for (j = i+1; j < n; j++)
sum -= A[i][j]*b[j];
b[i]=sum/A[i][i];
}
return 0;
}
/**
* Calculates the cross product of two 4-vectors
*
* @param x input 1
* @param y input 2
* @param T output
*
*/
template <class T>
static inline void Cross(const T *x, const T *y, T *result)
{
result[0] = x[1] * y[2] - x[2] * y[1];
result[1] = x[2] * y[0] - x[0] * y[2];
result[2] = x[0] * y[1] - x[1] * y[0];
return;
}
/**
* converts a quaternion to Euler angels in the roll pitch yaw system
*/
static inline void QuatRPYEuler(const double *quat, double *euler)
{
double n = sqrt(quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3]);
double s = n > 0?2./(n*n):0.;
double m00, m10, m20, m21, m22;
double xs = quat[1]*s;
double ys = quat[2]*s;
double zs = quat[3]*s;
double wx = quat[0]*xs;
double wy = quat[0]*ys;
double wz = quat[0]*zs;
double xx = quat[1]*xs;
double xy = quat[1]*ys;
double xz = quat[1]*zs;
double yy = quat[2]*ys;
double yz = quat[2]*zs;
double zz = quat[3]*zs;
m00 = 1.0 - (yy + zz);
m22 = 1.0 - (xx + yy);
m10 = xy + wz;
m20 = xz - wy;
m21 = yz + wx;
euler[0] = atan2(m21,m22);
euler[1] = atan2(-m20,sqrt(m21*m21 + m22*m22));
euler[2] = atan2(m10,m00);
}
/**
* converts from Euler angels in the roll pitch yaw system to a quaternion
*/
static inline void RPYEulerQuat(const double *euler, double *quat)
{
double sphi = sin(euler[0]);
double stheta = sin(euler[1]);
double spsi = sin(euler[2]);
double cphi = cos(euler[0]);
double ctheta = cos(euler[1]);
double cpsi = cos(euler[2]);
double _r[3][3] = { //create rotational Matrix
{cpsi*ctheta, cpsi*stheta*sphi - spsi*cphi, cpsi*stheta*cphi + spsi*sphi},
{spsi*ctheta, spsi*stheta*sphi + cpsi*cphi, spsi*stheta*cphi - cpsi*sphi},
{ -stheta, ctheta*sphi, ctheta*cphi}
};
#define MY_MAX(a,b) (((a)>(b))?(a):(b))
double _w = sqrt(MY_MAX(0, 1 + _r[0][0] + _r[1][1] + _r[2][2]))/2.0;
double _x = sqrt(MY_MAX(0, 1 + _r[0][0] - _r[1][1] - _r[2][2]))/2.0;
double _y = sqrt(MY_MAX(0, 1 - _r[0][0] + _r[1][1] - _r[2][2]))/2.0;
double _z = sqrt(MY_MAX(0, 1 - _r[0][0] - _r[1][1] + _r[2][2]))/2.0;
quat[0] = _w;
quat[1] = (_r[2][1] - _r[1][2])>=0?fabs(_x):-fabs(_x);
quat[2] = (_r[0][2] - _r[2][0])>=0?fabs(_y):-fabs(_y);
quat[3] = (_r[1][0] - _r[0][1])>=0?fabs(_z):-fabs(_z);
}
inline void transform3(const double *alignxf, double *point)
{
double x_neu, y_neu, z_neu;
x_neu = point[0] * alignxf[0] + point[1] * alignxf[4] + point[2] * alignxf[8];
y_neu = point[0] * alignxf[1] + point[1] * alignxf[5] + point[2] * alignxf[9];
z_neu = point[0] * alignxf[2] + point[1] * alignxf[6] + point[2] * alignxf[10];
point[0] = x_neu + alignxf[12];
point[1] = y_neu + alignxf[13];
point[2] = z_neu + alignxf[14];
}
inline void transform3(const double *alignxf, const double *point, double *tpoint)
{
tpoint[0] = point[0] * alignxf[0] + point[1] * alignxf[4] + point[2] * alignxf[8] + alignxf[12];
tpoint[1] = point[0] * alignxf[1] + point[1] * alignxf[5] + point[2] * alignxf[9] + alignxf[13];
tpoint[2] = point[0] * alignxf[2] + point[1] * alignxf[6] + point[2] * alignxf[10] + alignxf[14];
}
inline std::string trim(const std::string& source)
{
unsigned int start = 0, end = source.size() - 1;
while(source[start] == ' ') start++;
while(source[end] == ' ') end--;
return source.substr(start, end - start + 1);
}
#endif
Reference in a new issue Copy permalink