447 lines
10 KiB
C++
447 lines
10 KiB
C++
/* This file is part of the Pangolin Project.
|
|
* http://github.com/stevenlovegrove/Pangolin
|
|
*
|
|
* Copyright (c) 2011 Steven Lovegrove
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person
|
|
* obtaining a copy of this software and associated documentation
|
|
* files (the "Software"), to deal in the Software without
|
|
* restriction, including without limitation the rights to use,
|
|
* copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the
|
|
* Software is furnished to do so, subject to the following
|
|
* conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be
|
|
* included in all copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
|
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
|
|
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
|
|
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
|
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
|
|
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
|
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
|
|
* OTHER DEALINGS IN THE SOFTWARE.
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <iostream>
|
|
#include <string.h>
|
|
#include <algorithm>
|
|
#include <stdarg.h>
|
|
#include <cmath>
|
|
|
|
namespace pangolin
|
|
{
|
|
|
|
static const double Identity3d[] = {1,0,0, 0,1,0, 0,0,1};
|
|
static const double Zero3d[] = {0,0,0, 0,0,0, 0,0,0};
|
|
static const double Identity4d[] = {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1};
|
|
static const double Zero4d[] = {0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0};
|
|
|
|
static const float Identity3f[] = {1,0,0, 0,1,0, 0,0,1};
|
|
static const float Zero3f[] = {0,0,0, 0,0,0, 0,0,0};
|
|
static const float Identity4f[] = {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1};
|
|
static const float Zero4f[] = {0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0};
|
|
|
|
template<int R, int C, typename P>
|
|
void MatPrint(const P m[R*C])
|
|
{
|
|
for( int r=0; r < R; ++r)
|
|
{
|
|
for( int c=0; c < C; ++c )
|
|
std::cout << m[R*c+r] << " ";
|
|
std::cout << std::endl;
|
|
}
|
|
std::cout << std::endl;
|
|
}
|
|
|
|
template<int R, int C, typename P>
|
|
void MatPrint(const P m[R*C], std::string name)
|
|
{
|
|
std::cout << name << " = [" << std::endl;
|
|
for( int r=0; r < R; ++r)
|
|
{
|
|
for( int c=0; c < C; ++c )
|
|
std::cout << m[R*c+r] << " ";
|
|
std::cout << std::endl;
|
|
}
|
|
std::cout << std::endl << "]" << std::endl;
|
|
}
|
|
|
|
// Set array using varadic arguments
|
|
template<int R, int C, typename P>
|
|
void MatSet(P m[R*C], ...)
|
|
{
|
|
va_list ap;
|
|
va_start(ap,m);
|
|
for( int i=0; i< R*C; ++i )
|
|
{
|
|
*m = (P)va_arg(ap,double);
|
|
++m;
|
|
}
|
|
}
|
|
|
|
// m = zeroes(N)
|
|
template<int R, int C, typename P>
|
|
void SetZero(P m[R*C] )
|
|
{
|
|
std::fill_n(m,R*C,0);
|
|
}
|
|
|
|
// m = identity(N)
|
|
template<int N, typename P>
|
|
void SetIdentity(P m[N*N] )
|
|
{
|
|
std::fill_n(m,N*N,0);
|
|
for( int i=0; i< N; ++i )
|
|
m[N*i+i] = 1;
|
|
}
|
|
|
|
// mo = m1 * m2
|
|
template<int R, int M, int C, typename P>
|
|
void MatMul(P mo[R*C], const P m1[R*M], const P m2[M*C] )
|
|
{
|
|
for( int r=0; r < R; ++r)
|
|
for( int c=0; c < C; ++c )
|
|
{
|
|
mo[R*c+r] = 0;
|
|
for( int m=0; m < M; ++ m) mo[R*c+r] += m1[R*m+r] * m2[M*c+m];
|
|
}
|
|
}
|
|
|
|
// mo = m1 * m2 * s
|
|
template<int R, int M, int C, typename P>
|
|
void MatMul(P mo[R*C], const P m1[R*M], const P m2[M*C], P s )
|
|
{
|
|
for( int r=0; r < R; ++r)
|
|
for( int c=0; c < C; ++c )
|
|
{
|
|
mo[R*c+r] = 0;
|
|
for( int m=0; m < M; ++ m) mo[R*c+r] += m1[R*m+r] * m2[M*c+m] * s;
|
|
}
|
|
}
|
|
|
|
// mo = m1 * transpose(m2)
|
|
template<int R, int M, int C, typename P>
|
|
void MatMulTranspose(P mo[R*C], const P m1[R*M], const P m2[C*M] )
|
|
{
|
|
for( int r=0; r < R; ++r)
|
|
for( int c=0; c < C; ++c )
|
|
{
|
|
mo[R*c+r] = 0;
|
|
for( int m=0; m < M; ++ m) mo[R*c+r] += m1[R*m+r] * m2[C*m+c];
|
|
}
|
|
}
|
|
|
|
// m = m1 + m2
|
|
template<int R, int C, typename P>
|
|
void MatAdd(P m[R*C], const P m1[R*C], const P m2[R*C])
|
|
{
|
|
for( int i=0; i< R*C; ++i )
|
|
m[i] = m1[i] + m2[i];
|
|
}
|
|
|
|
// m = m1 - m2
|
|
template<int R, int C, typename P>
|
|
void MatSub(P m[R*C], const P m1[R*C], const P m2[R*C])
|
|
{
|
|
for( int i=0; i< R*C; ++i )
|
|
m[i] = m1[i] - m2[i];
|
|
}
|
|
|
|
// m = m1 * scalar
|
|
template<int R, int C, typename P>
|
|
void MatMul(P m[R*C], const P m1[R*C], P scalar)
|
|
{
|
|
for( int i=0; i< R*C; ++i )
|
|
m[i] = m1[i] * scalar;
|
|
}
|
|
|
|
// m = m1 + m2
|
|
template<int R, int C, typename P>
|
|
void MatMul(P m[R*C], P scalar)
|
|
{
|
|
for( int i=0; i< R*C; ++i )
|
|
m[i] *= scalar;
|
|
}
|
|
|
|
template<int N, typename P>
|
|
void MatTranspose(P out[N*N], const P in[N*N] )
|
|
{
|
|
for( int c=0; c<N; ++c )
|
|
for( int r=0; r<N; ++r )
|
|
out[N*c+r] = in[N*r+c];
|
|
}
|
|
|
|
template<int N, typename P>
|
|
void MatTranspose(P m[N*N] )
|
|
{
|
|
for( int c=0; c<N; ++c )
|
|
for( int r=0; r<c; ++r )
|
|
std::swap<P>(m[N*c+r],m[N*r+c]);
|
|
}
|
|
|
|
// m = a x b
|
|
template<typename P>
|
|
void VecCross3(P m[3], const P a[3], const P b[3])
|
|
{
|
|
m[0] = a[1]*b[2] - a[2]*b[1];
|
|
m[1] = a[2]*b[0] - a[0]*b[2];
|
|
m[2] = a[0]*b[1] - a[1]*b[0];
|
|
}
|
|
|
|
// s = skewSymetrixMatrix(v)
|
|
template<typename P>
|
|
void MatSkew(P s[3*3], const P v[3] )
|
|
{
|
|
s[0] = 0;
|
|
s[1] = v[2];
|
|
s[2] = -v[1];
|
|
s[3] = -v[2];
|
|
s[4] = 0;
|
|
s[5] = v[0];
|
|
s[6] = v[1];
|
|
s[7] = -v[0];
|
|
s[8] = 0;
|
|
}
|
|
|
|
template<int N, typename P>
|
|
void MatOrtho( P m[N*N] )
|
|
{
|
|
// http://www.euclideanspace.com/maths/algebra/matrix/orthogonal/index.htm
|
|
P Itimes3[N*N];
|
|
SetIdentity<N>(Itimes3);
|
|
MatMul<N,N>(Itimes3,(P)3.0);
|
|
|
|
P mmT[N*N];
|
|
MatMulTranspose<N,N,N>(mmT,m,m);
|
|
|
|
P _c[N*N];
|
|
MatSub<N,N>(_c,Itimes3,mmT);
|
|
P _m[N*N];
|
|
MatMul<N,N,N>(_m,m,_c,(P)0.5);
|
|
std::copy(_m,_m+(N*N),m);
|
|
}
|
|
|
|
template<typename P>
|
|
void Rotation(P R[3*3], P x, P y, P z)
|
|
{
|
|
P sx = sin(x);
|
|
P sy = sin(y);
|
|
P sz = sin(z);
|
|
P cx = cos(x);
|
|
P cy = cos(y);
|
|
P cz = cos(z);
|
|
// P cx = sqrt( (P)1.0 - sx * sx);
|
|
// P cy = sqrt( (P)1.0 - sy * sy);
|
|
// P cz = sqrt( (P)1.0 - sz * sz);
|
|
R[0] = cy * cz;
|
|
R[1] = sx * sy * cz + cx * sz;
|
|
R[2] = -cx * sy * cz + sx * sz;
|
|
R[3] = -cy * sz;
|
|
R[4] = -sx * sy * sz + cx * cz;
|
|
R[5] = cx * sy * sz + sx * cz;
|
|
R[6] = sy;
|
|
R[7] = -sx * cy;
|
|
R[8] = cx * cy;
|
|
}
|
|
|
|
template<typename P>
|
|
void LieSetIdentity(P T_ba[3*4] )
|
|
{
|
|
SetIdentity<3>(T_ba);
|
|
std::fill_n(T_ba+(3*3),3,0);
|
|
}
|
|
|
|
template<typename P>
|
|
void LieSetRotation(P T_ba[3*4], const P R_ba[3*3] )
|
|
{
|
|
std::copy(R_ba,R_ba+(3*3),T_ba);
|
|
}
|
|
|
|
template<typename P>
|
|
void LieSetTranslation(P T_ba[3*4], const P a_b[3] )
|
|
{
|
|
std::copy(a_b, a_b+3, T_ba+(3*3));
|
|
}
|
|
|
|
template<typename P>
|
|
void LieSetSE3(P T_ba[3*4], const P R_ba[3*3], const P a_b[3] )
|
|
{
|
|
LieSetRotation<P>(T_ba,R_ba);
|
|
LieSetTranslation<P>(T_ba,a_b);
|
|
}
|
|
|
|
template<typename P>
|
|
void LieGetRotation(P R_ba[3*3], const P T_ba[3*4] )
|
|
{
|
|
std::copy(T_ba,T_ba+(3*3),R_ba);
|
|
}
|
|
|
|
template<typename P>
|
|
void LieApplySO3( P out[3], const P R_ba[3*3], const P in[3] )
|
|
{
|
|
MatMul<3,3,1,P>(out,R_ba,in);
|
|
}
|
|
|
|
template<typename P>
|
|
void LieApplySE3vec( P x_b[3], const P T_ba[3*4], const P x_a[3] )
|
|
{
|
|
P rot[3];
|
|
MatMul<3,3,1,P>(rot,T_ba,x_a);
|
|
MatAdd<3,1,P>(x_b,rot,T_ba+(3*3));
|
|
}
|
|
|
|
template<typename P>
|
|
void LieApplySE34x4vec3( P x_b[3], const P T_ba[4*4], const P x_a[3] )
|
|
{
|
|
x_b[0] = T_ba[0]*x_a[0] + T_ba[4]*x_a[1] + T_ba[8]*x_a[2] + T_ba[12];
|
|
x_b[1] = T_ba[1]*x_a[0] + T_ba[5]*x_a[1] + T_ba[9]*x_a[2] + T_ba[13];
|
|
x_b[2] = T_ba[2]*x_a[0] + T_ba[6]*x_a[1] + T_ba[10]*x_a[2] + T_ba[14];
|
|
}
|
|
|
|
template<typename P>
|
|
void LieMulSO3( P R_ca[3*3], const P R_cb[3*3], const P R_ba[3*3] )
|
|
{
|
|
MatMul<3,3,3>(R_ca,R_cb,R_ba);
|
|
}
|
|
|
|
template<typename P>
|
|
void LieMulSE3( P T_ca[3*4], const P T_cb[3*4], const P T_ba[3*4] )
|
|
{
|
|
LieMulSO3<>(T_ca,T_cb,T_ba);
|
|
P R_cb_times_a_b[3];
|
|
LieApplySO3<>(R_cb_times_a_b,T_cb,T_ba+(3*3));
|
|
MatAdd<3,1>(T_ca+(3*3),R_cb_times_a_b,T_cb+(3*3));
|
|
}
|
|
|
|
template<typename P>
|
|
void LiePutSE3in4x4(P out[4*4], const P in[3*4] )
|
|
{
|
|
SetIdentity<4>(out);
|
|
std::copy(in,in+3, out);
|
|
std::copy(in+3,in+6, out+4);
|
|
std::copy(in+6,in+9, out+8);
|
|
std::copy(in+9,in+12, out+12);
|
|
}
|
|
|
|
template<typename P>
|
|
void LieSE3from4x4(P out[3*4], const P in[4*4] )
|
|
{
|
|
std::copy(in,in+3, out);
|
|
std::copy(in+4,in+7, out+3);
|
|
std::copy(in+8,in+11, out+6);
|
|
std::copy(in+12,in+15, out+9);
|
|
}
|
|
|
|
template<typename P>
|
|
void LieMul4x4bySE3( P T_ca[4*4], const P T_cb[3*4], const P T_ba[4*4] )
|
|
{
|
|
// TODO: fast
|
|
P T_cb4[4*4];
|
|
LiePutSE3in4x4<>(T_cb4,T_cb);
|
|
P res[4*4];
|
|
MatMul<4,4,4>(res,T_cb4,T_ba);
|
|
std::copy(res,res+(4*4),T_ca);
|
|
}
|
|
|
|
template<typename P>
|
|
void LieTransposeSO3( P R_ab[3*3], const P R_ba[3*3] )
|
|
{
|
|
MatTranspose<3,P>(R_ab,R_ba);
|
|
}
|
|
|
|
template<typename P>
|
|
void LieInverseSE3( P T_ab[3*4], const P T_ba[3*4] )
|
|
{
|
|
LieTransposeSO3<P>(T_ab,T_ba);
|
|
P minus_b_a[3];
|
|
LieApplySO3(minus_b_a, T_ab, T_ba+(3*3));
|
|
MatMul<3,1,P>(T_ab+(3*3),minus_b_a, -1);
|
|
}
|
|
|
|
// c = a x b
|
|
template<typename P>
|
|
void CrossProduct( P c[3], const P a[3], const P b[3] )
|
|
{
|
|
c[0] = a[1] * b[2] - a[2] * b[1];
|
|
c[1] = a[2] * b[0] - a[0] * b[2];
|
|
c[2] = a[0] * b[1] - a[1] * b[0];
|
|
}
|
|
|
|
template<int R, typename P>
|
|
P Length( P v[R] )
|
|
{
|
|
P sum_sq = 0;
|
|
|
|
for(size_t r = 0; r < R; ++r ) {
|
|
sum_sq += v[r] * v[r];
|
|
}
|
|
|
|
return sqrt(sum_sq);
|
|
}
|
|
|
|
|
|
template<int R, typename P>
|
|
void Normalise( P v[R] )
|
|
{
|
|
const P length = Length<R,P>(v);
|
|
|
|
for(size_t r = 0; r < R; ++r ) {
|
|
v[r] /= length;
|
|
}
|
|
}
|
|
|
|
template<typename P>
|
|
void EnforceUpT_wc(P T_wc[3*4], const P up_w[3])
|
|
{
|
|
// Copy R_wc
|
|
P R_wc[3*3];
|
|
std::copy(T_wc,T_wc+3*3,R_wc);
|
|
|
|
// New R_wc should go into T_wc
|
|
P* NR_wc = T_wc;
|
|
|
|
// // cx_w,cy_w,cz_w are camera axis in world coordinates
|
|
// // Calculate new camera coordinates (ncx_w,ncy_w,ncz_w)
|
|
|
|
// ncx_w = up_w x cz_w
|
|
CrossProduct(NR_wc + 0*3, up_w, R_wc + 2*3);
|
|
|
|
// ncy_w = cz_w x ncx_w
|
|
CrossProduct(NR_wc + 1*3, R_wc + 2*3, NR_wc + 0*3);
|
|
|
|
// ncz_w = cz_w
|
|
std::copy(R_wc + 2*3, R_wc + 3*3, NR_wc + 2*3);
|
|
|
|
Normalise<3,P>(NR_wc + 0*3);
|
|
Normalise<3,P>(NR_wc + 1*3);
|
|
Normalise<3,P>(NR_wc + 2*3);
|
|
}
|
|
|
|
template<typename P>
|
|
void EnforceUpT_cw(P T_cw_4x4[4*4], const P up_w[3])
|
|
{
|
|
// 3x4 from 4x4
|
|
P T_cw[3*4];
|
|
LieSE3from4x4<P>(T_cw,T_cw_4x4);
|
|
|
|
// Invert T_cw
|
|
P T_wc[3*4];
|
|
LieInverseSE3<P>(T_wc, T_cw);
|
|
|
|
// Enforce up for T_wc
|
|
EnforceUpT_wc<P>(T_wc, up_w);
|
|
|
|
// Invert
|
|
LieInverseSE3<P>(T_cw, T_wc);
|
|
|
|
// 4x4 from 3x4
|
|
LiePutSE3in4x4<P>(T_cw_4x4,T_cw);
|
|
}
|
|
|
|
}
|