tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix(*): fix EigenSolver bug

Browse Source
This commit is contained in:
Messier 2019-09-04 17:03:22 +08:00
parent d0fbd5b1cd
commit 7747a0c3fc
3 changed files with 79 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
include/Ctain/Matrix.h
View File

@ -123,6 +123,18 @@ namespace Ctain {
return res;
}
friend Matrix<_Scalar> operator *(
const Matrix<_Scalar> &m, double a) {
Matrix<_Scalar> res;
res = m;
for(int i = 0; i < m._Rows; i++) {
for(int j = 0; j < m._Cols; j++) {
res.Data(i,j) *= a;
}
}
return res;
}
friend Matrix<_Scalar> operator -(
const Matrix<_Scalar> &m) {
Matrix<_Scalar> res;
@ -268,6 +280,14 @@ namespace Ctain {
template<typename _Scalar>
void Matrix<_Scalar>::operator =(Matrix<_Scalar> m) {
if(m._isSub) {
if(_isSub) {
for (int i = 0; i < m._Rows; i++) {
for (int j = 0; j < m._Cols; j++) {
Data(i,j) = m.cData(i, j);
}
}
return;
}
_isSub = true;
_Rows = m._Rows;
_Cols = m._Cols;
@ -352,7 +372,8 @@ namespace Ctain {
template<typename _Scalar>
Matrix<_Scalar> Matrix<_Scalar>::operator /(double m) const {
Matrix<_Scalar> res(_Rows, _Cols);
Matrix<_Scalar> res;
res = *this;
for(int i = 0; i < _Rows; i++) {
for(int j = 0; j < _Cols; j++) {
res.Data(i,j) /= m;

69
include/Ctain/MatrixSolver.h
View File

@ -7,20 +7,30 @@
#include <cmath>
#include <complex>
static bool Matrix_EigenValue(double *K1,int n,int LoopNumber,double Error1,double *Ret);
static void Matrix_Hessenberg(double *A1,int n,double *ret);
namespace Ctain {
class EigenSolver {
public:
EigenSolver(SMatrix<double> s) {
_matrix = s;
Matrix<double> tt(_matrix.rows(),2);
Matrix_EigenValue(_matrix.addr(),_matrix.rows(),1000,1e-10,tt.addr());
double *A = new double[s.rows()*2];
double *B = new double[s.size()];
for(int i = 0; i < s.size(); i++)
B[i] = s(i);
memset(A, 0, sizeof(s.rows()*2));
Matrix_EigenValue(B, s.rows(),1000,1e-10,A);
Matrix<double> tt(A, s.rows(), 2);
t=tt;
std::cout<<"s:"<<s;
SMatrix<double> s2(A, s.rows());
std::cout<<"tt:"<<tt;
std::cout<<"s2:"<<s2;
delete []A;
delete []B;
}
Matrix<double> eigenvalues() {
return t;
}
private:
SMatrix<double> _matrix;
Matrix<double> t;
};
@ -29,56 +39,58 @@ namespace Ctain {
static void Matrix_Hessenberg(double *A1,int n,double *ret)
{
int i,j,k,MaxNumber;
int MaxNumber;
double temp,*A;
A=new double[n*n];
for (i=0;i<n;i++) {
k=i*n;
for (j=0;j<n;j++)
memset(A, 0, sizeof(double)*n*n);
for (int i=0;i<n;i++) {
int k=i*n;
for (int j=0;j<n;j++)
{
A[k+j]=A1[k+j];
}
}
for (k=1;k<n-1;k++) {
i=k-1;
for (int k=1;k<n-1;k++) {
int i=k-1;
MaxNumber=k;
temp=abs(A[k*n+i]);
for (j=k+1;j<n;j++) {
if (abs(A[j*n+i])>temp) {
temp=abs(A[j*n+i]);
temp=fabs(A[k*n+i]);
for (int j=k+1;j<n;j++) {
if (fabs(A[j*n+i])>temp) {
temp=fabs(A[j*n+i]);
MaxNumber=j;
}
}
ret[0]=A[MaxNumber*n+i];
i=MaxNumber;
if (ret[0]!=0) {
if (i!=k) {
for(j=k-1;j<n;j++) {
if (MaxNumber!=k) {
for(int j=k-1;j<n;j++) {
temp=A[i*n+j];
A[i*n+j]=A[k*n+j];
A[k*n+j]=temp;
}
for(j=0;j<n;j++) {
for(int j=0;j<n;j++) {
temp=A[j*n+i];
A[j*n+i]=A[j*n+k];
A[j*n+k]=temp;
}
}
for (i=k+1;i<n;i++) {
for (int i=k+1;i<n;i++) {
temp=A[i*n+k-1]/ret[0];
A[i*n+k-1]=0;
for (j=k;j<n;j++) {
for (int j=k;j<n;j++) {
A[i*n+j]-=temp*A[k*n+j];
}
for (j=0;j<n;j++) {
for (int j=0;j<n;j++) {
A[j*n+k]+=temp*A[j*n+i];
}
}
}
}
for (i=0;i<n;i++) {
k=i*n;
for (j=0;j<n;j++) {
for (int i=0;i<n;i++) {
int k=i*n;
for (int j=0;j<n;j++) {
ret[k+j]=A[k+j];
}
}
@ -90,16 +102,17 @@ static bool Matrix_EigenValue(double *K1,int n,int LoopNumber,double Error1,doub
int i,j,k,t,m,Loop1;
double b,c,d,g,xy,p,q,r,x,s,e,f,z,y,temp,*A;
A=new double[n*n];
memset(A, 0, sizeof(double)*n*n);
Matrix_Hessenberg(K1,n,A);
m=n;
Loop1=LoopNumber;
while(m!=0) {
t=m-1;
while(t>0) {
temp=abs(A[(t-1)*n+t-1]);
temp+=abs(A[t*n+t]);
temp=fabs(A[(t-1)*n+t-1]);
temp+=fabs(A[t*n+t]);
temp=temp*Error1;
if (abs(A[t*n+t-1])>temp) {
if (fabs(A[t*n+t-1])>temp) {
t--;
}
else {
@ -116,7 +129,7 @@ static bool Matrix_EigenValue(double *K1,int n,int LoopNumber,double Error1,doub
b=-A[(m-1)*n+m-1]-A[(m-2)*n+m-2];
c=A[(m-1)*n+m-1]*A[(m-2)*n+m-2]-A[(m-1)*n+m-2]*A[(m-2)*n+m-1];
d=b*b-4*c;
y=sqrt(abs(d));
y=sqrt(fabs(d));
if (d>0) {
xy=1;
if (b<0) {

20
src/mynteye/api/camera_models/equidistant_camera.cc
View File

@ -651,20 +651,25 @@ void EquidistantCamera::backprojectSymmetric(
if (npow >= 9) {
coeffs(9) = mParameters.k5();
}
std::cout << std::endl << std::endl << "coeffs:" << coeffs;
if (npow == 1) {
theta = p_u_norm;
} else {
// Get eigenvalues of companion matrix corresponding to polynomial.
// Eigenvalues correspond to roots of polynomial.
Ctain::MatrixXd A(npow, npow);
Ctain::Matrixd A(npow);
A.setZero();
A.block(1, 0, npow - 1, npow - 1).setIdentity();
A.col(npow - 1) = -coeffs.block(0, 0, npow, 1) / coeffs(npow);
std::cout << std::endl <<"A:" << A;
Ctain::EigenSolver es(A);
Ctain::MatrixXcd eigval = es.eigenvalues();
Ctain::Matrix<double> eigval(9, 2);
eigval = es.eigenvalues();
// Ctain::EigenSolver es(A);
// Ctain::MatrixXcd eigval(npow, 2);
// eigval = es.eigenvalues();
std::cout << std::endl <<"eigval:" << eigval;
std::vector<double> thetas;
for (int i = 0; i < eigval.rows(); ++i) {
if (fabs(eigval(i, 1)) > tol) { //imag
@ -684,10 +689,17 @@ void EquidistantCamera::backprojectSymmetric(
if (thetas.empty()) {
theta = p_u_norm;
std::cout<<std::endl<<"empty";
} else {
theta = *std::min_element(thetas.begin(), thetas.end());
// theta = 1.3457661;
std::cout<<"thetas[]:";
for(auto t:thetas)
std::cout<<t<<" ";
}
}
std::cout << std::endl <<"thetas:" << theta <<" phi:"<<phi;
}