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
Download Patch File Download Diff File Expand all files Collapse all files

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;
}