void StateEstimatorKinematic::dare(const Eigen::Matrix<double,6,6> &A, const Eigen::Matrix<double,6,6> &B, Eigen::Matrix<double,6,6> &P,int zDim)
{
Eigen::Matrix<double,6,6> Ainv = A.inverse();
Eigen::Matrix<double,6,6> ABRB;
if (zDim == 6)
{
ABRB = Ainv * B * _R.llt().solve(B.transpose());
}
else {
ABRB = Ainv * B.topLeftCorner(6,zDim) * _R.topLeftCorner(zDim,zDim).llt().solve(B.topLeftCorner(6,zDim).transpose());
}
Eigen::Matrix<double,2*6,2*6> Z;
Z.block(0,0,6,6) = Ainv;
Z.block(0,6,6,6) = ABRB;
Z.block(6,0,6,6) = _Q * Ainv;
Z.block(6,6,6,6) = A.transpose() + _Q * ABRB;
Eigen::ComplexEigenSolver <Eigen::Matrix<double,2*6,2*6> > ces;
ces.compute(Z);
Eigen::Matrix<std::complex<double>,2*6,1> eigVal = ces.eigenvalues();
Eigen::Matrix<std::complex<double>,2*6,2*6> eigVec = ces.eigenvectors();
Eigen::Matrix<std::complex<double>,2*6,6> unstableEigVec;
int ctr = 0;
for (int i = 0; i < 2*6; i++) {
if (eigVal(i).real()*eigVal(i).real() + eigVal(i).imag()*eigVal(i).imag() > 1) {
unstableEigVec.col(ctr) = eigVec.col(i);
ctr++;
if (ctr > 6)
break;
}
}
Eigen::Matrix<std::complex<double>,6,6> U21inv = unstableEigVec.block(0,0,6,6).inverse();
Eigen::Matrix<std::complex<double>,6,6> PP = unstableEigVec.block(6,0,6,6) * U21inv;
for (int i = 0; i < 6; i++) {
for (int j = 0; j < 6; j++) {
P(i,j) = PP(i,j).real();
}
}
}
void dare(const Eigen::Matrix<double,xDim,xDim> &A, const Eigen::Matrix<double,xDim,uDim> &B, Eigen::Matrix<double,xDim,xDim> &P)
{
Eigen::Matrix<double,xDim,xDim> Ainv = A.inverse();
Eigen::Matrix<double,xDim,xDim> ABRB = Ainv * B * _R.llt().solve(B.transpose());
Eigen::Matrix<double,2*xDim,2*xDim> Z;
Z.block(0,0,xDim,xDim) = Ainv;
Z.block(0,xDim,xDim,xDim) = ABRB;
Z.block(xDim,0,xDim,xDim) = _Q * Ainv;
Z.block(xDim,xDim,xDim,xDim) = A.transpose() + _Q * ABRB;
Eigen::ComplexEigenSolver <Eigen::Matrix<double,2*xDim,2*xDim> > ces;
ces.compute(Z);
Eigen::Matrix<std::complex<double>,2*xDim,1> eigVal = ces.eigenvalues();
Eigen::Matrix<std::complex<double>,2*xDim,2*xDim> eigVec = ces.eigenvectors();
Eigen::Matrix<std::complex<double>,2*xDim,xDim> unstableEigVec;
int ctr = 0;
for (int i = 0; i < 2*xDim; i++) {
if (eigVal(i).real()*eigVal(i).real() + eigVal(i).imag()*eigVal(i).imag() > 1) {
unstableEigVec.col(ctr) = eigVec.col(i);
ctr++;
if (ctr > xDim)
break;
}
}
Eigen::Matrix<std::complex<double>,xDim,xDim> U21inv = unstableEigVec.block(0,0,xDim,xDim).inverse();
Eigen::Matrix<std::complex<double>,xDim,xDim> PP = unstableEigVec.block(xDim,0,xDim,xDim) * U21inv;
for (int i = 0; i < xDim; i++) {
for (int j = 0; j < xDim; j++) {
P(i,j) = PP(i,j).real();
}
}
}
int main()
{
float width = 640.0f, height=480.0f;
char buf[255];
int n, pose;
AsfMatrix data;
Eigen::Matrix<float,40*6,-1> Shapes; //240x116
float mean_size = 0;
for(n=0;n<40;n++)
{
for(pose=0;pose<6;pose++)
{
sprintf(buf,"../../../CIL/data/imm_face_db/%.2d-%dm.asf",n+1,pose+1);
if(readAsf2Eigen(buf, data) != 0)
{
sprintf(buf,"../../../CIL/data/imm_face_db/%.2d-%df.asf",n+1,pose+1);
if (readAsf2Eigen(buf, data) != 0)
continue;
}
//Initialize The Shapes Container
if(Shapes.cols() == 0)
Shapes.resize(40*6,data.cols()*2);
//Copy the found data
Shapes.block(n*6+pose,0,1,data.cols()) = data.row(2) * width;
Shapes.block(n*6+pose,data.cols(),1,data.cols()) = data.row(3) * height;
//Compute MeanShape
auto mean_x = Shapes.block(n*6+pose,0,1,data.cols()).mean();
auto mean_y = Shapes.block(n*6+pose,data.cols(),1,data.cols()).mean();
auto mshape_x = (Shapes.block(n*6+pose,0,1,data.cols()).array()-mean_x).pow(2) ;
auto mshape_y = (Shapes.block(n*6+pose,data.cols(),1,data.cols()).array()-mean_y).pow(2) ;
mean_size += sqrt((mshape_x+mshape_y).sum());
//std::cout << Shapes.block(n*pose+pose,0,1,data.cols()) << std::endl;
// std::cout << Shapes.block(0,0,1,5) << std::endl ;
//std::cout << Shapes.block(1,0,1,5) << std::endl ;
//std::cout << Shapes.block(2,0,1,5) << std::endl ;
//std::cout << Shapes.block(3,0,1,5) << std::endl << std::endl;
}
}
mean_size /= 40*6;
int number_of_landmarks = data.cols();
int number_of_shapes = Shapes.rows();
//Complex notation and Substracting Mean.
Eigen::MatrixXcf X(number_of_shapes, number_of_landmarks);
X.real() = Shapes.leftCols(number_of_landmarks);
X.imag() = Shapes.rightCols(number_of_landmarks);
X.array().colwise() -= X.rowwise().mean().array();
//Eigen::MatrixXcd XX(10,10);
//double test[10] = {0};
//Eigen::Map<Eigen::MatrixXd> mat(test, 10, 1);
Eigen::MatrixXcf C;
Eigen::MatrixXcf Mean;
cil::alg::gpa(X,C,Mean);
std::cout << X.rows() << " , " << X.cols() << std::endl<< std::endl;
std::cout << C.rows() << " , " << C.cols() << std::endl<< std::endl;
std::cout << Mean.rows() << " , " << Mean.cols() << std::endl<< std::endl;
std::cout << C.row(1).transpose() << std::endl<< std::endl;
return 0;
X.array().colwise() -= X.rowwise().mean().array();
//Eigen::MatrixXcf X = Shapes.block(0,0,Shapes.rows(),data.cols()) * std::complex<float>(0,1) +
// Shapes.block(0,data.cols(),Shapes.rows(),data.cols())*std::complex<float>(1,0);
//Eigen::VectorXcf Mean = X.rowwise().mean();
//std::complex<float> *m_ptr = Mean.data();
//for(n=0;n<Mean.rows();++n)
// X.row(n) = X.row(n).array() - *m_ptr++;
//Solve Eigen Problem
Eigen::MatrixXcf A = X.transpose().conjugate() * X;
Eigen::ComplexEigenSolver<Eigen::MatrixXcf> solver;
solver.compute(A);
// std::cout << "The Eigenvales of A are:" << std::endl << solver.eigenvalues() <<std::endl<<std::endl;
// std::complex<float> lambda = solver.eigenvalues()[57];
// std::cout << "Consider the first eigenvalue, lambda = " << lambda << std::endl;
// std::cout << "EigenVec for largest EigenVals of A are:" << std::endl << solver.eigenvectors().col(57) <<std::endl<<std::endl;
auto eigvec_mean = solver.eigenvectors().col(solver.eigenvectors().cols()-1);
// Full Procrusters fits
Eigen::MatrixXcf f = (X * eigvec_mean).array() / (X * X.transpose().conjugate()).diagonal().array();
//Transform
auto f_conj = f.conjugate().array();
for(n=0;n<X.cols();++n)
X.col(n) = X.col(n).array() * f_conj;
auto mf = f.mean();
std::cout << mf << std::endl<< std::endl;
mf = mf / sqrt(mf.real()*mf.real()+mf.imag()*mf.imag());
std::cout << mf << std::endl<< std::endl;
auto m = eigvec_mean * mf;
X = X*mf;
std::cout << X.row(0).transpose() << std::endl<< std::endl;
return 0;
}