SVD for Mat3
This commit is contained in:
parent
a4c422f32a
commit
731b2a128d
27
t.cc
27
t.cc
@ -2786,6 +2786,12 @@ cout <<"resorted graph = "<<adjperm<<endl;
|
||||
|
||||
if(1)
|
||||
{
|
||||
int seed;
|
||||
int f=open("/dev/random",O_RDONLY);
|
||||
if(sizeof(int)!=read(f,&seed,sizeof(int))) laerror("cannot read /dev/random");
|
||||
close(f);
|
||||
srand(seed);
|
||||
|
||||
Mat3<double> a;
|
||||
a.randomize(10.);
|
||||
cout<<a<<endl;
|
||||
@ -2794,22 +2800,37 @@ Mat3<double> work(ata);
|
||||
Vec3<double> w;
|
||||
Mat3<double> v;
|
||||
work.eivec_sym(w,v,true);
|
||||
cout <<w<<endl<<sqrt(w[0])<<" "<<sqrt(w[1])<<" "<<sqrt(w[2])<<" "<<endl<<endl<<v<<endl;
|
||||
//cout <<w<<endl<<sqrt(w[0])<<" "<<sqrt(w[1])<<" "<<sqrt(w[2])<<" "<<endl<<endl<<v<<endl;
|
||||
|
||||
Mat3<double> aat = a*a.transpose();
|
||||
Mat3<double> work2(aat);
|
||||
Vec3<double> w2;
|
||||
Mat3<double> v2;
|
||||
work2.eivec_sym(w2,v2,true);
|
||||
cout <<w2<<endl<<sqrt(w2[0])<<" "<<sqrt(w2[1])<<" "<<sqrt(w2[2])<<" "<<endl<<endl<<v2<<endl;
|
||||
//cout <<w2<<endl<<sqrt(w2[0])<<" "<<sqrt(w2[1])<<" "<<sqrt(w2[2])<<" "<<endl<<endl<<v2<<endl;
|
||||
//cout <<"dets "<<v2.determinant()<<" "<<v.determinant()<<endl;
|
||||
|
||||
Mat3<double> u3,v3;
|
||||
Vec3<double> w3;
|
||||
a.svd(u3,w3,v3);
|
||||
cout <<"Mat3 SVD\n";
|
||||
cout <<u3<<endl<<w3<<endl<<endl<<v3.transpose()<<endl<<endl;
|
||||
cout <<"Mat3 SVD dets "<<u3.determinant()<<" "<<v3.determinant()<<endl;
|
||||
|
||||
|
||||
NRMat<double> aa(a);
|
||||
cout <<aa.nrows()<<" "<<aa.ncols()<<endl;
|
||||
NRMat<double> uu(3,3),vv(3,3);
|
||||
NRVec<double> ss(3);
|
||||
singular_decomposition(aa,&uu,ss,&vv,0);
|
||||
cout <<"REGULAR SVD\n";
|
||||
cout <<uu<<ss<<vv<<endl;
|
||||
|
||||
cout <<"svd dets "<<determinant(uu)<<" "<<determinant(vv)<<endl;
|
||||
|
||||
Mat3<double> ainv= a.inverse();
|
||||
Mat3<double> ainv2= a.svdinverse();
|
||||
cout <<endl<<"Inverse via svd\n"<<ainv2<<endl;
|
||||
cout <<"Difference of inverses = "<<(ainv-ainv2).norm()<<endl;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
150
vecmat3.cc
150
vecmat3.cc
@ -662,12 +662,6 @@ else for(int i=0;i<3;++i) for(int j=0; j<3; ++j) q[i][j]=x*RANDDOUBLESIGNED();
|
||||
#define NO_NUMERIC_LIMITS
|
||||
#endif
|
||||
|
||||
#ifdef NO_NUMERIC_LIMITS
|
||||
#define DBL_EPSILON 1.19209290e-07f
|
||||
#else
|
||||
#define DBL_EPSILON std::numeric_limits<T>::epsilon()
|
||||
#endif
|
||||
|
||||
#define M_SQRT3 1.73205080756887729352744634151 // sqrt(3)
|
||||
#define SQR(x) ((x)*(x)) // x^2
|
||||
//
|
||||
@ -947,6 +941,150 @@ T Mat3<T>::norm(const T scalar) const
|
||||
return sqrt(sum);
|
||||
}
|
||||
|
||||
//efficient SVD for 3x3 matrices cf. Mc.Adams et al. UWM technical report 1690 (2011)
|
||||
//
|
||||
|
||||
|
||||
template<typename T>
|
||||
static inline T ABS(const T a)
|
||||
{
|
||||
return a<0?-a:a;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
static inline T MAX(const T a, const T b)
|
||||
{
|
||||
return a>b ? a : b;
|
||||
}
|
||||
|
||||
|
||||
|
||||
template<typename T>
|
||||
void QRGivensQuaternion(T a1, T a2, T &ch, T &sh)
|
||||
{
|
||||
// a1 = pivot point on diagonal
|
||||
// a2 = lower triangular entry we want to annihilate
|
||||
T rho = sqrt(a1*a1 + a2*a2);
|
||||
|
||||
sh = rho > DBL_EPSILON ? a2 : 0;
|
||||
ch = ABS(a1) + MAX(rho,DBL_EPSILON);
|
||||
if(a1 < 0)
|
||||
{
|
||||
T tmp=sh;
|
||||
sh=ch;
|
||||
ch=tmp;
|
||||
}
|
||||
T w = (T)1/sqrt(ch*ch+sh*sh);
|
||||
ch *= w;
|
||||
sh *= w;
|
||||
}
|
||||
|
||||
|
||||
template<typename T>
|
||||
void Mat3<T>::qrd(Mat3 &Q, Mat3 &R)
|
||||
{
|
||||
|
||||
T ch1,sh1,ch2,sh2,ch3,sh3;
|
||||
T a,b;
|
||||
|
||||
// first givens rotation (ch,0,0,sh)
|
||||
QRGivensQuaternion(q[0][0],q[1][0],ch1,sh1);
|
||||
a=1-2*sh1*sh1;
|
||||
b=2*ch1*sh1;
|
||||
// apply B = Q' * B
|
||||
R[0][0]= a*q[0][0]+b*q[1][0]; R[0][1]= a*q[0][1]+b*q[1][1]; R[0][2]= a*q[0][2]+b*q[1][2];
|
||||
R[1][0]= -b*q[0][0]+a*q[1][0]; R[1][1]= -b*q[0][1]+a*q[1][1]; R[1][2]= -b*q[0][2]+a*q[1][2];
|
||||
R[2][0]= q[2][0]; R[2][1]= q[2][1]; R[2][2]= q[2][2];
|
||||
|
||||
// second givens rotation (ch,0,-sh,0)
|
||||
QRGivensQuaternion(R[0][0],R[2][0],ch2,sh2);
|
||||
a= 1-2*sh2*sh2;
|
||||
b= 2*ch2*sh2;
|
||||
// apply B = Q' * B;
|
||||
q[0][0]= a*R[0][0]+b*R[2][0]; q[0][1]= a*R[0][1]+b*R[2][1]; q[0][2]= a*R[0][2]+b*R[2][2];
|
||||
q[1][0]= R[1][0]; q[1][1]= R[1][1]; q[1][2]= R[1][2];
|
||||
q[2][0]= -b*R[0][0]+a*R[2][0]; q[2][1]= -b*R[0][1]+a*R[2][1]; q[2][2]= -b*R[0][2]+a*R[2][2];
|
||||
|
||||
// third givens rotation (ch,sh,0,0)
|
||||
QRGivensQuaternion(q[1][1],q[2][1],ch3,sh3);
|
||||
a= 1-2*sh3*sh3;
|
||||
b= 2*ch3*sh3;
|
||||
// R is now set to desired value
|
||||
R[0][0]= q[0][0]; R[0][1]= q[0][1]; R[0][2]= q[0][2];
|
||||
R[1][0]= a*q[1][0]+b*q[2][0]; R[1][1]= a*q[1][1]+b*q[2][1]; R[1][2]= a*q[1][2]+b*q[2][2];
|
||||
R[2][0]= -b*q[1][0]+a*q[2][0]; R[2][1]= -b*q[1][1]+a*q[2][1]; R[2][2]= -b*q[1][2]+a*q[2][2];
|
||||
|
||||
// construct the cumulative rotation Q=Q1 * Q2 * Q3
|
||||
// the number of floating point operations for three quaternion multiplications
|
||||
// is more or less comparable to the explicit form of the joined matrix.
|
||||
// certainly more memory-efficient!
|
||||
T sh12= sh1*sh1;
|
||||
T sh22= sh2*sh2;
|
||||
T sh32= sh3*sh3;
|
||||
|
||||
Q[0][0]= (-1+2*sh12)*(-1+2*sh22);
|
||||
Q[0][1]= 4*ch2*ch3*(-1+2*sh12)*sh2*sh3+2*ch1*sh1*(-1+2*sh32);
|
||||
Q[0][2]= 4*ch1*ch3*sh1*sh3-2*ch2*(-1+2*sh12)*sh2*(-1+2*sh32);
|
||||
|
||||
Q[1][0]= 2*ch1*sh1*(1-2*sh22);
|
||||
Q[1][1]= -8*ch1*ch2*ch3*sh1*sh2*sh3+(-1+2*sh12)*(-1+2*sh32);
|
||||
Q[1][2]= -2*ch3*sh3+4*sh1*(ch3*sh1*sh3+ch1*ch2*sh2*(-1+2*sh32));
|
||||
|
||||
Q[2][0]= 2*ch2*sh2;
|
||||
Q[2][1]= 2*ch3*(1-2*sh22)*sh3;
|
||||
Q[2][2]= (-1+2*sh22)*(-1+2*sh32);
|
||||
}
|
||||
|
||||
|
||||
template<typename T>
|
||||
void Mat3<T>::svd(Mat3 &u, Vec3<T> &w, Mat3 &v, bool proper_rotations) const
|
||||
{
|
||||
Mat3 ata= this->Ttimes(*this);
|
||||
Vec3<T> ww; //actually not needed squares of singular values
|
||||
ata.eivec_sym(ww,v,true);
|
||||
Mat3 uw = (*this)*v;
|
||||
Mat3 r;
|
||||
uw.qrd(u,r);
|
||||
w[0]= r[0][0];
|
||||
w[1]= r[1][1];
|
||||
w[2]= r[2][2];
|
||||
|
||||
if(proper_rotations) return;
|
||||
if(w[2]<0)
|
||||
{
|
||||
w[2] = -w[2];
|
||||
u[0][2] = -u[0][2];
|
||||
u[1][2] = -u[1][2];
|
||||
u[2][2] = -u[2][2];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
template<typename T>
|
||||
void Mat3<T>::diagmultl(const Vec3<T> &rhs)
|
||||
{
|
||||
for(int i=0; i<3; ++i) for(int j=0; j<3; ++j) q[i][j] *= rhs[i];
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
void Mat3<T>::diagmultr(const Vec3<T> &rhs)
|
||||
{
|
||||
for(int i=0; i<3; ++i) for(int j=0; j<3; ++j) q[i][j] *= rhs[j];
|
||||
}
|
||||
|
||||
|
||||
template<typename T>
|
||||
const Mat3<T> Mat3<T>::svdinverse(const T thr) const
|
||||
{
|
||||
Mat3 u,v;
|
||||
Vec3<T> w;
|
||||
svd(u,w,v);
|
||||
for(int i=0; i<3; ++i) w[i] = (w[i]<thr)? 0 : 1/w[i];
|
||||
v.diagmultr(w);
|
||||
return v.timesT(u);
|
||||
}
|
||||
|
||||
|
||||
//cf. https://en.wikipedia.org/wiki/3D_projection
|
||||
template<typename T>
|
||||
void perspective(T *proj_xy, const Vec3<T> &point, const Mat3<T> &rot_angle, const Vec3<T> &camera, const Vec3<T> &plane_to_camera)
|
||||
|
||||
13
vecmat3.h
13
vecmat3.h
@ -33,6 +33,14 @@
|
||||
|
||||
namespace LA_Vecmat3 {
|
||||
|
||||
#ifdef NO_NUMERIC_LIMITS
|
||||
#define DBL_EPSILON 1.19209290e-07f
|
||||
#else
|
||||
#define DBL_EPSILON std::numeric_limits<T>::epsilon()
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
float fast_sqrtinv(float);
|
||||
|
||||
//forward declaration
|
||||
@ -172,6 +180,11 @@ public:
|
||||
void eival_sym(Vec3<T> &w, const bool sortdown=false) const; //only for real symmetric matrix, symmetry is not checked
|
||||
void eivec_sym(Vec3<T> &w, Mat3 &v, const bool sortdown=false) const; //only for real symmetric matrix, symmetry is not checked
|
||||
T norm(const T scalar = 0) const;
|
||||
void qrd(Mat3 &q, Mat3 &r); //not const, destroys the matrix
|
||||
void svd(Mat3 &u, Vec3<T> &w, Mat3 &v, bool proper_rotations=false) const; //if proper_rotations = true, singular value can be negative but u and v are proper rotations
|
||||
void diagmultl(const Vec3<T> &rhs);
|
||||
void diagmultr(const Vec3<T> &rhs);
|
||||
const Mat3 svdinverse(const T thr=1000*DBL_EPSILON) const;
|
||||
};
|
||||
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user