v01
This commit is contained in:
Ivan
446
thirdparty/Pangolin/include/pangolin/utils/simple_math.h
vendored
Normal file
446
thirdparty/Pangolin/include/pangolin/utils/simple_math.h
vendored
Normal file
@@ -0,0 +1,446 @@
|
||||
/* 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);
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user