示例#1
0
void ctms_decompositions()
{
  const int maxSize = 16;
  const int size    = 12;

  typedef Eigen::Matrix<Scalar,
                        Eigen::Dynamic, Eigen::Dynamic,
                        0,
                        maxSize, maxSize> Matrix;

  typedef Eigen::Matrix<Scalar,
                        Eigen::Dynamic, 1,
                        0,
                        maxSize, 1> Vector;

  typedef Eigen::Matrix<std::complex<Scalar>,
                        Eigen::Dynamic, Eigen::Dynamic,
                        0,
                        maxSize, maxSize> ComplexMatrix;

  const Matrix A(Matrix::Random(size, size));
  const ComplexMatrix complexA(ComplexMatrix::Random(size, size));
  const Matrix saA = A.adjoint() * A;

  // Cholesky module
  Eigen::LLT<Matrix>  LLT;  LLT.compute(A);
  Eigen::LDLT<Matrix> LDLT; LDLT.compute(A);

  // Eigenvalues module
  Eigen::HessenbergDecomposition<ComplexMatrix> hessDecomp;        hessDecomp.compute(complexA);
  Eigen::ComplexSchur<ComplexMatrix>            cSchur(size);      cSchur.compute(complexA);
  Eigen::ComplexEigenSolver<ComplexMatrix>      cEigSolver;        cEigSolver.compute(complexA);
  Eigen::EigenSolver<Matrix>                    eigSolver;         eigSolver.compute(A);
  Eigen::SelfAdjointEigenSolver<Matrix>         saEigSolver(size); saEigSolver.compute(saA);
  Eigen::Tridiagonalization<Matrix>             tridiag;           tridiag.compute(saA);

  // LU module
  Eigen::PartialPivLU<Matrix> ppLU; ppLU.compute(A);
  Eigen::FullPivLU<Matrix>    fpLU; fpLU.compute(A);

  // QR module
  Eigen::HouseholderQR<Matrix>        hQR;  hQR.compute(A);
  Eigen::ColPivHouseholderQR<Matrix>  cpQR; cpQR.compute(A);
  Eigen::FullPivHouseholderQR<Matrix> fpQR; fpQR.compute(A);

  // SVD module
  Eigen::JacobiSVD<Matrix> jSVD; jSVD.compute(A, ComputeFullU | ComputeFullV);
}
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();
        }
      }
    }
示例#4
0
文件: main.cpp 项目: pksorensen/CIL
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;
}