void homographyToPoseCV(at::real fx, at::real fy,
at::real tagSize,
const at::Mat& horig,
cv::Mat& rvec,
cv::Mat& tvec) {
at::Point3 opoints[4] = {
at::Point3(-1, -1, 0),
at::Point3( 1, -1, 0),
at::Point3( 1, 1, 0),
at::Point3(-1, 1, 0)
};
at::Point ipoints[4];
at::real s = 0.5*tagSize;
for (int i=0; i<4; ++i) {
ipoints[i] = project(horig, opoints[i].x, opoints[i].y);
opoints[i] *= s;
}
at::real Kdata[9] = {
fx, 0, 0,
0, fy, 0,
0, 0, 1
};
at::Mat Kmat(3, 3, Kdata);
at::Mat dcoeffs = at::Mat::zeros(4, 1);
cv::Mat_<at::Point3> omat(4, 1, opoints);
cv::Mat_<at::Point> imat(4, 1, ipoints);
cv::Mat r, t;
cv::solvePnP(omat, imat, Kmat, dcoeffs, r, t);
if (rvec.type() == CV_32F) {
r.convertTo(rvec, rvec.type());
} else {
rvec = r;
}
if (tvec.type() == CV_32F) {
t.convertTo(tvec, tvec.type());
} else {
tvec = t;
}
}
void krylov(const ComplexSparseMatrixType &A, ComplexVectorType &Vec,
const ComplexType Prefactor, const size_t Kmax)
{
// INFO(A);
const RealType beta_err = 1.0E-12;
if (DEBUG) assert( Kmax > 2 );
RealType alpha;
RealType beta = 1.0;
ComplexMatrixType Vm = ComplexMatrixType::Zero(Vec.size(), Kmax);
std::vector<RealType> Alphas;
std::vector<RealType> Betas;
int cntK = 0;
//NOTE: normalized Vec
Vec.normalize();
Vm.col(cntK) = Vec;
while ( cntK < Kmax ) {
ComplexVectorType work = A * Vm.col(cntK);
if( cntK > 0 ) work -= beta * Vm.col(cntK-1);
ComplexType alpha_c = work.dot( Vm.col(cntK) );
alpha = alpha_c.real();
work -= alpha * Vm.col(cntK);
beta = work.norm();
Alphas.push_back(alpha);
if( DEBUG > 4 ){
INFO("@ " << cntK << " alpha is " << alpha << " beta is " << beta);
}
if( beta > beta_err ){
work.normalize();
if( cntK+1 < Kmax ) {
Vm.col(cntK+1) = work;
Betas.push_back(beta);
}
cntK++;
}
else{
cntK++;
break;
}
}
if ( DEBUG ) {
assert( Alphas.size() == cntK );
assert( Betas.size() == cntK - 1 );
if ( DEBUG > 5 ) {
ComplexMatrixType tmpVm = Vm;
tmpVm.adjointInPlace();
INFO( "Vm^H * Vm " << std::endl << tmpVm * Vm);
}
}
int Kused = cntK;
if ( Kused == 1 ){
/* NOTE: This is a special case that input vector is eigenvector */
Vec = exp(Prefactor * Alphas.at(0)) * Vec;
} else{
/* NOTE: The floowing codes use Eigen to solve the tri-diagonal matrix.
If we use this, we need the Eigen header in this file.
*/
// RealMatrixType TriDiag = RealMatrixType::Zero(Kused, Kused);
// for (size_t cnt = 0; cnt < Kused; cnt++) {
// TriDiag(cnt, cnt) = Alphas.at(cnt);
// if (cnt > 0) {
// TriDiag(cnt, cnt-1) = Betas.at(cnt - 1);
// TriDiag(cnt-1, cnt) = Betas.at(cnt - 1);
// }
// }
// Eigen::SelfAdjointEigenSolver<RealMatrixType> es;
// es.compute(TriDiag);
// RealVectorType Dvec = es.eigenvalues();
// ComplexMatrixType Dmat = ComplexMatrixType::Zero(Kused, Kused);
// for (size_t cnt = 0; cnt < Kused; cnt++) {
// Dmat(cnt,cnt) = exp( Prefactor * Dvec(cnt) );
// }
// RealMatrixType Kmat = es.eigenvectors();
/* NOTE: The floowing codes use MKL Lapack to solve the tri-diagonal matrix */
RealType* d = &Alphas[0];
RealType* e = &Betas[0];
RealType* z = (RealType*)malloc(Kused * Kused * sizeof(RealType));
RealType* work = (RealType*)malloc(4 * Kused * sizeof(RealType));
int info;
//dstev - LAPACK
dstev((char*)"V", &Kused, d, e, z, &Kused, work, &info);
Eigen::Map<RealMatrixType> Kmat(z, Kused, Kused);
Kmat.transposeInPlace();
ComplexMatrixType Dmat = ComplexMatrixType::Zero(Kused, Kused);
for (size_t cnt = 0; cnt < Kused; cnt++) {
Dmat(cnt,cnt) = exp( Prefactor * d[cnt] );
}
if(info != 0){
INFO("Lapack INFO = " << info);
RUNTIME_ERROR("Error in Lapack function 'dstev'");
}
/* NOTE: After Solving tri-diagonal matrix, we need Kmat and Dmat to proceed further. */
if ( DEBUG > 4 ) {
RealMatrixType tmpKmat = Kmat;
tmpKmat.transposeInPlace();
INFO( Kmat * Dmat * tmpKmat );
}
ComplexMatrixType Otmp = Vm.block(0, 0, Vec.size(), Kused) * Kmat;
Vec = ( Otmp * Dmat ) * ( Otmp.adjoint() * Vec );
}
}
