C++ SelfAdjointEigenSolver::compute方法代码示例

double
RotationAverage::chordal(Sophus::SO3d *avg)
{
    double max_angle=0;
    Eigen::Matrix4d Q;
    Q.setZero();
    auto rest = rotations.begin();
    rest++;
    for(auto && t: zip_range(weights, rotations))
    {
        double w=t.get<0>();
        Sophus::SO3d& q = t.get<1>();
        Q += w * q.unit_quaternion().coeffs() * q.unit_quaternion().coeffs().transpose();
        max_angle = std::accumulate(rest, rotations.end(), max_angle,
                [&q](double max, const Sophus::SO3d& other)
                {
                return std::max(max,
                    q.unit_quaternion().angularDistance(other.unit_quaternion()));
                });
    }
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> solver;
    solver.compute(Q);
    Eigen::Vector4d::Map(avg->data()) = solver.eigenvectors().col(3);
    return max_angle;
 }

void Foam::mosDMDEigenBase::realSymmEigenSolver(const  Eigen::MatrixXd& M, Eigen::DiagonalMatrix<scalar, Eigen::Dynamic>& S, Eigen::MatrixXd& U)
{    
    // Solve eigenvalues and eigenvectors
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver;
    eigenSolver.compute(M);
      
    // Sort eigenvalues and corresponding eigenvectors
    // in descending order
    SortableList<scalar> sortedList(M.rows());

    forAll (sortedList, i)
    {
        sortedList[i] = eigenSolver.eigenvalues()[i];
    }
         
    // Do sort 
    sortedList.sort();
      
    label n = 0;
    forAllReverse(sortedList, i)
    {
        S.diagonal()[n] = sortedList[i];
        U.col(n) = eigenSolver.eigenvectors().col(sortedList.indices()[i]);
        
        n++;
    }

/// Draw ellipsoid
void QGLVisualizer::drawEllipsoid(const Vec3& pos, const Mat33& covariance) const{
    Eigen::SelfAdjointEigenSolver<Mat33> es;
    es.compute(covariance);
    Mat33 V(es.eigenvectors());
    double GLmat[16]={V(0,0), V(1,0), V(2,0), 0, V(0,1), V(1,1), V(2,1), 0, V(0,2), V(1,2), V(2,2), 0, pos.x(), pos.y(), pos.z(), 1};
    glPushMatrix();
        glMultMatrixd(GLmat);
        drawEllipsoid(10,10,sqrt(es.eigenvalues()(0))*config.ellipsoidScale, sqrt(es.eigenvalues()(1))*config.ellipsoidScale, sqrt(es.eigenvalues()(2))*config.ellipsoidScale);
    glPopMatrix();
}

void computeEigenReferenceSolution
(
	size_t nr_problems,
	const Vcl::Core::InterleavedArray<float, 3, 3, -1>& ATA,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>& U,
	Vcl::Core::InterleavedArray<float, 3, 1, -1>& S
)
{
	// Compute reference using Eigen
	for (int i = 0; i < static_cast<int>(nr_problems); i++)
	{
		Vcl::Matrix3f A = ATA.at<float>(i);

		Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver;
		solver.compute(A, Eigen::ComputeEigenvectors);

		U.at<float>(i) = solver.eigenvectors();
		S.at<float>(i) = solver.eigenvalues();
	}
}

void
drawBoundingBox(PointCloudT::Ptr cloud,
				boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer,
				int z)
{
	
	//Eigen::Vector4f centroid;
	pcl::compute3DCentroid(*cloud, centroid);
	
	//Eigen::Matrix3f covariance;
	computeCovarianceMatrixNormalized(*cloud, centroid, covariance);
	//Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance,
	//Eigen::ComputeEigenvectors);

	eigen_solver.compute(covariance,Eigen::ComputeEigenvectors);
	
//	eigen_solver = boost::shared_ptr<Eigen::SelfAdjointEigenSolver>
//		(covariance,Eigen::ComputeEigenvectors);

	eigDx = eigen_solver.eigenvectors();
    eigDx.col(2) = eigDx.col(0).cross(eigDx.col(1));


	//Eigen::Matrix4f p2w(Eigen::Matrix4f::Identity());
	p2w.block<3,3>(0,0) = eigDx.transpose();
	p2w.block<3,1>(0,3) = -1.f * (p2w.block<3,3>(0,0) * centroid.head<3>());
    //pcl::PointCloud<PointT> cPoints;
    pcl::transformPointCloud(*cloud, cPoints, p2w);


	//PointT min_pt, max_pt;
    pcl::getMinMax3D(cPoints, min_pt, max_pt);
    const Eigen::Vector3f mean_diag = 0.5f*(max_pt.getVector3fMap() + min_pt.getVector3fMap());

	const Eigen::Quaternionf qfinal(eigDx);
    const Eigen::Vector3f tfinal = eigDx*mean_diag + centroid.head<3>();
	
	//viewer->addPointCloud(cloud);
	viewer->addCube(tfinal, qfinal, max_pt.x - min_pt.x, max_pt.y - min_pt.y, max_pt.z - min_pt.z,boost::lexical_cast<std::string>((z+1)*200));

}

std::list< LieGroup > confidence_region_contours(const NormalDistributionOn<LieGroup>& N, int dim, typename LieGroup::Scalar confidence) {
	using namespace std;
	
	list< typename LieGroup::Tangent > sphere = sample_sphere< typename LieGroup::Scalar, LieGroup::DoF >(50, dim);
	
	boost::math::chi_squared_distribution<typename LieGroup::Scalar> chi2( LieGroup::DoF );
	double scale = sqrt( boost::math::quantile(chi2, confidence) ) ;
	
	Eigen::SelfAdjointEigenSolver< typename NormalDistributionOn< LieGroup >::Covariance > eigs;
	eigs.compute(N.covariance());
	
	typename NormalDistributionOn<SE2d>::Covariance sqrt_cov = eigs.eigenvectors() * eigs.eigenvalues().array().sqrt().matrix().asDiagonal();
	
	std::list< LieGroup > out;
	
	for( typename list< typename LieGroup::Tangent >::iterator it = sphere.begin(); it != sphere.end(); it++ ) {
		out.push_back( N.mean() * LieGroup::exp( scale* sqrt_cov * (*it) ) );
	}
	
	return out;
	
}

typename GaussianProcess<TScalarType>::MatrixType GaussianProcess<TScalarType>::InvertKernelMatrix(const typename GaussianProcess<TScalarType>::MatrixType &K,
                                                      typename GaussianProcess<TScalarType>::InversionMethod inv_method = GaussianProcess<TScalarType>::FullPivotLU,
                                                                                                   bool stable) const{
    // compute core matrix
    if(debug){
        std::cout << "GaussianProcess::InvertKernelMatrix: inverting kernel matrix... ";
        std::cout.flush();
    }

    typename GaussianProcess<TScalarType>::MatrixType core;

    switch(inv_method){
    // standard method: fast but not that accurate
    // Uses the LU decomposition with full pivoting for the inversion
    case FullPivotLU:{
        if(debug) std::cout << " (inversion method: FullPivotLU) " << std::flush;
        try{
            if(stable){
                core = K.inverse();
            }
            else{
                if(debug) std::cout << " (using lapack) " << std::flush;
                core = lapack::lu_invert<TScalarType>(K);
            }
        }
        catch(lapack::LAPACKException& e){
            core = K.inverse();
        }
    }
    break;

    // very accurate and very slow method, use it for small problems
    // Uses the two-sided Jacobi SVD decomposition
    case JacobiSVD:{
        if(debug) std::cout << " (inversion method: JacobiSVD) " << std::flush;
        Eigen::JacobiSVD<MatrixType> jacobisvd(K, Eigen::ComputeThinU | Eigen::ComputeThinV);
        if((jacobisvd.singularValues().real().array() < 0).any() && debug){
            std::cout << "GaussianProcess::InvertKernelMatrix: warning: there are negative eigenvalues.";
            std::cout.flush();
        }
        core = jacobisvd.matrixV() * VectorType(1/jacobisvd.singularValues().array()).asDiagonal() * jacobisvd.matrixU().transpose();
    }
    break;

    // accurate method and faster than Jacobi SVD.
    // Uses the bidiagonal divide and conquer SVD
    case BDCSVD:{
        if(debug) std::cout << " (inversion method: BDCSVD) " << std::flush;
#ifdef EIGEN_BDCSVD_H
        Eigen::BDCSVD<MatrixType> bdcsvd(K, Eigen::ComputeThinU | Eigen::ComputeThinV);
        if((bdcsvd.singularValues().real().array() < 0).any() && debug){
            std::cout << "GaussianProcess::InvertKernelMatrix: warning: there are negative eigenvalues.";
            std::cout.flush();
        }
        core = bdcsvd.matrixV() * VectorType(1/bdcsvd.singularValues().array()).asDiagonal() * bdcsvd.matrixU().transpose();
#else
        // this is checked, since BDCSVD is currently not in the newest release
        throw std::string("GaussianProcess::InvertKernelMatrix: BDCSVD is not supported by the provided Eigen library.");
#endif

    }
    break;

    // faster than the SVD method but less stable
    // computes the eigenvalues/eigenvectors of selfadjoint matrices
    case SelfAdjointEigenSolver:{
        if(debug) std::cout << " (inversion method: SelfAdjointEigenSolver) " << std::flush;
        try{
            core = lapack::chol_invert<TScalarType>(K);
        }
        catch(lapack::LAPACKException& e){
            Eigen::SelfAdjointEigenSolver<MatrixType> es;
            es.compute(K);
            VectorType eigenValues = es.eigenvalues().reverse();
            MatrixType eigenVectors = es.eigenvectors().rowwise().reverse();
            if((eigenValues.real().array() < 0).any() && debug){
                std::cout << "GaussianProcess::InvertKernelMatrix: warning: there are negative eigenvalues.";
                std::cout.flush();
            }
            core = eigenVectors * VectorType(1/eigenValues.array()).asDiagonal() * eigenVectors.transpose();
        }
    }
    break;
    }

    if(debug) std::cout << "[done]" << std::endl;
    return core;
}

  int Fit(Vector& res_G,  // residual under NULL -- may change when permuting
          Vector& v_G,    // variance under NULL -- may change when permuting
          Matrix& X_G,    // covariance
          Matrix& G_G,    // genotype
          Vector& w_G)    // weight
  {
    this->nPeople = X_G.rows;
    this->nMarker = G_G.cols;
    this->nCovariate = X_G.cols;

    // calculation w_sqrt
    G_to_Eigen(w_G, &this->w_sqrt);
    w_sqrt = w_sqrt.cwiseSqrt();

    // calculate K = G * W * G'
    Eigen::MatrixXf G;
    G_to_Eigen(G_G, &G);
    this->K_sqrt.noalias() = w_sqrt.asDiagonal() * G.transpose();

    // calculate Q = ||res * K||
    Eigen::VectorXf res;
    G_to_Eigen(res_G, &res);
    this->Q = (this->K_sqrt * res).squaredNorm();

    // calculate P0 = V - V X (X' V X)^(-1) X' V
    Eigen::VectorXf v;
    G_to_Eigen(v_G, &v);
    if (this->nCovariate == 1) {
      P0 = -v * v.transpose() / v.sum();
      // printf("dim(P0) = %d, %d\n", P0.rows(), P0.cols());
      // printf("dim(v) = %d\n", v.size());
      P0.diagonal() += v;
      // printf("dim(v) = %d\n", v.size());
    } else {
      Eigen::MatrixXf X;
      G_to_Eigen(X_G, &X);
      Eigen::MatrixXf XtV;  // X^t V
      XtV.noalias() = X.transpose() * v.asDiagonal();
      P0 = -XtV.transpose() * ((XtV * X).inverse()) * XtV;
      P0.diagonal() += v;
    }
    // dump();
    // Eigen::MatrixXf tmp = K_sqrt * P0 * K_sqrt.transpose();
    // dumpToFile(tmp, "out.tmp");
    // eigen decomposition
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es;
    es.compute(K_sqrt * P0 * K_sqrt.transpose());

#ifdef DEBUG
    std::ofstream k("K");
    k << K_sqrt;
    k.close();
#endif
    // std::ofstream p("P0");
    // p << P0;
    // p.close();

    this->mixChiSq.reset();
    int r_ub = std::min(nPeople, nMarker);
    int r = 0;  // es.eigenvalues().size();
    int eigen_len = es.eigenvalues().size();
    for (int i = eigen_len - 1; i >= 0; i--) {
      if (es.eigenvalues()[i] > ZBOUND && r < r_ub) {
        this->mixChiSq.addLambda(es.eigenvalues()[i]);
        r++;
      } else
        break;
    }
    // calculate p-value
    this->pValue = this->mixChiSq.getPvalue(this->Q);
    if (this->pValue == 0.0 || this->pValue == 1.0) {
      this->pValue = this->mixChiSq.getLiuPvalue(this->Q);
    }
    return 0;
  };

void PointWithNormalStatistcsGenerator::computeNormalsAndSVD(PointWithNormalVector& points, PointWithNormalSVDVector& svds, const Eigen::MatrixXi& indices, 
							     const Eigen::Matrix3f& cameraMatrix, const Eigen::Isometry3f& transform){
  _integralImage.compute(indices,points);
  int q=0;
  int outerStep = _numThreads * _step;
  PixelMapper pixelMapper;
  pixelMapper.setCameraMatrix(cameraMatrix);
  pixelMapper.setTransform(transform);
  Eigen::Isometry3f inverseTransform = transform.inverse();
  #pragma omp parallel
  {
#ifdef _PWN_USE_OPENMP_
    int threadNum = omp_get_thread_num();
#else // _PWN_USE_OPENMP_
    int threadNum = 0; 
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver;
#endif // _PWN_USE_OPENMP_

    for (int c=threadNum; c<indices.cols(); c+=outerStep) {
#ifdef _PWN_USE_OPENMP_
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver;
#endif // _PWN_USE_OPENMP_
      for (int r=0; r<indices.rows(); r+=_step, q++){
	int index  = indices(r,c);
	//cerr << "index("  << r <<"," << c << ")=" << index <<  endl;
	if (index<0)
	  continue;
	// determine the region
	PointWithNormal& point = points[index];
	PointWithNormalSVD& originalSVD = svds[index];
	PointWithNormalSVD svd;
	Eigen::Vector3f normal = point.normal();
	Eigen::Vector3f coord = pixelMapper.projectPoint(point.point()+Eigen::Vector3f(_worldRadius, _worldRadius, 0));
	svd._z=point(2);

	coord.head<2>()*=(1./coord(2));
	int dx = abs(c - coord[0]);
	int dy = abs(r - coord[1]);
	if (dx>_imageRadius)
	  dx = _imageRadius;
	if (dy>_imageRadius)
	  dy = _imageRadius;
	PointAccumulator acc = _integralImage.getRegion(c-dx, c+dx, r-dy, r+dy);
	svd._mean=point.point();
	if (acc.n()>_minPoints){
	  Eigen::Vector3f mean = acc.mean();
	  svd._mean = mean;
	  svd._n = acc.n();
	  Eigen::Matrix3f cov  = acc.covariance();
	  eigenSolver.compute(cov);
	  svd._U=eigenSolver.eigenvectors();
	  svd._singularValues=eigenSolver.eigenvalues();
	  if (svd._singularValues(0) <0)
	    svd._singularValues(0) = 0;
	  /*
	    cerr << "\t svd.singularValues():" << svd.singularValues() << endl;
	    cerr << "\t svd.U():" << endl << svd.U() << endl;
	    //cerr << "\t svd.curvature():" << svd.curvature() << endl;
	
	    */
	

	  normal = eigenSolver.eigenvectors().col(0).normalized();
	  if (normal.dot(inverseTransform * mean) > 0.0f)
	    normal =-normal;
	  svd.updateCurvature();
	  //cerr << "n(" << index << ") c:"  << svd.curvature() << endl << point.tail<3>() << endl;
	  if (svd.curvature()>_maxCurvature){
	    //cerr << "region: " << c-dx << " " <<  c+dx << " " <<  r-dx << " " << r+dx << " points: " << acc.n() << endl;
	    normal.setZero();
	  } 
	} else {
	  normal.setZero();
	  svd = PointWithNormalSVD();
	}
	if (svd.n() > originalSVD.n()){
	  originalSVD = svd;
	  point.setNormal(normal);
	}
      } 
    }
  }
}

//.........这里部分代码省略.........
          *ysub_start = *trace_start;
          ysub_start++;
          sample_offset++;
        }
        trace_start += col_step;
        filt_start += col_step;
      }
    }
  }

  // Copy in all the data into working matrix
  Y.resize(loc_num_wells, (int)num_frames);
  for (int frame_ix = 0; frame_ix < (int)num_frames; frame_ix++) {
    for (int row_ix = row_start; row_ix < row_end; row_ix++) {
      size_t store_offset = row_ix * num_cols + col_start;
      int16_t *trace_start = data + (flow_frame_stride * frame_ix) + (flow_ix * frame_stride) + store_offset;
      int16_t *trace_end = trace_start + loc_num_cols;
      float * y_start = Y.data() + loc_num_wells * frame_ix + (row_ix - row_start) * loc_num_cols;
      while( trace_start != trace_end ) {
        *y_start++ = *trace_start++;
      }
    }
  }
  Eigen::VectorXf col_mean = Y.colwise().sum();
  col_mean /= Y.rows();

  for (int i = 0; i < Y.cols(); i++) {
    Y.col(i).array() -= col_mean.coeff(i);
  }
  // Create scatter matrix
  S = Ysub.transpose() * Ysub;
  // Compute the eigenvectors
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es;
  es.compute(S);
  Eigen::MatrixXf Pca_Basis = es.eigenvectors();
  Eigen::VectorXf Pca_Values = es.eigenvalues();
  // Copy top eigen vectors into basis for projection
  Basis.resize(num_frames, num_pca);
  for (int i = 0; i < Basis.cols(); i++) {
    //    Basis.col(i) = es.eigenvectors().col(es.eigenvectors().cols() - i -1);
    Basis.col(i) = Pca_Basis.col(Pca_Basis.cols() - i - 1);
  }
  // Create solver matrix, often not a good way of solving things but eigen vectors should be stable and fast
  Eigen::MatrixXf SX = (Basis.transpose() * Basis).inverse() * Basis.transpose();
  // Get coefficients to solve
  Eigen::MatrixXf B = Y * SX.transpose();
  // Uncompress data into yhat matrix
  Eigen::MatrixXf Yhat = B * Basis.transpose();

  for (int i = 0; i < Yhat.cols(); i++) {
    Yhat.col(i).array() += col_mean.coeff(i);
    Y.col(i).array() += col_mean.coeff(i);
  }
  
  // H5File h5("pca_lossy.h5");
  // h5.Open();
  // char buff[256];
  // snprintf(buff, sizeof(buff), "/Y_%d_%d_%d", flow_ix, row_start, col_start);
  // H5Eigen::WriteMatrix(h5, buff, Y);
  // snprintf(buff, sizeof(buff), "/Yhat_%d_%d_%d", flow_ix, row_start, col_start);
  // H5Eigen::WriteMatrix(h5, buff, Yhat);
  // snprintf(buff, sizeof(buff), "/Basis_%d_%d_%d", flow_ix, row_start, col_start);
  // H5Eigen::WriteMatrix(h5, buff, Basis);
  // h5.Close();
  // Copy data out of yhat matrix into original data structure, keeping track of residuals
  for (int frame_ix = 0; frame_ix < (int)num_frames; frame_ix++) {

void AbsoluteOrientation::compute( std::vector<Eigen::Vector3d> &left, std::vector<Eigen::Vector3d> &right, Eigen::Quaterniond &result )
{
	int i, pairNum = left.size();

	Eigen::MatrixXd muLmuR = Eigen::MatrixXd::Zero(3,3), M = Eigen::MatrixXd::Zero(3,3), 
		curMat = Eigen::MatrixXd::Zero(3,3), N = Eigen::MatrixXd::Zero(4,4);

	Eigen::Vector3d meanFirst(0,0,0), meanSecond(0,0,0); //assume points set to zero by constructor

	//compute the mean of both point sets
	for (i=0; i<pairNum; i++) {
		meanFirst[0] += left[i][0];	    meanFirst[1] += left[i][1];	    meanFirst[2] += left[i][2];
		meanSecond[0] += right[i][0];	  meanSecond[1] += right[i][1];	  meanSecond[2] += right[i][2];
	}
	meanFirst[0]/=pairNum;	  meanFirst[1]/=pairNum;	  meanFirst[2]/=pairNum;
	meanSecond[0]/=pairNum;	  meanSecond[1]/=pairNum;	  meanSecond[2]/=pairNum;

	//compute the matrix muLmuR
	muLmuR(0,0) = meanFirst[0]*meanSecond[0];		
	muLmuR(0,1) = meanFirst[0]*meanSecond[1];		
	muLmuR(0,2) = meanFirst[0]*meanSecond[2];
	muLmuR(1,0) = meanFirst[1]*meanSecond[0];
	muLmuR(1,1) = meanFirst[1]*meanSecond[1];
	muLmuR(1,2) = meanFirst[1]*meanSecond[2];
	muLmuR(2,0) = meanFirst[2]*meanSecond[0];
	muLmuR(2,1) = meanFirst[2]*meanSecond[1];
	muLmuR(2,2) = meanFirst[2]*meanSecond[2];

	//compute the matrix M
	for (i=0; i<pairNum; i++) {
		Eigen::Vector3d &leftPoint = left[i];
		Eigen::Vector3d &rightPoint = right[i];
		curMat(0,0) = leftPoint[0]*rightPoint[0];		
		curMat(0,1) = leftPoint[0]*rightPoint[1];		
		curMat(0,2) = leftPoint[0]*rightPoint[2];
		curMat(1,0) = leftPoint[1]*rightPoint[0];
		curMat(1,1) = leftPoint[1]*rightPoint[1];
		curMat(1,2) = leftPoint[1]*rightPoint[2];
		curMat(2,0) = leftPoint[2]*rightPoint[0];
		curMat(2,1) = leftPoint[2]*rightPoint[1];
		curMat(2,2) = leftPoint[2]*rightPoint[2];
		M+=curMat;
	}
	M+= (muLmuR *(-pairNum));

	//compute the matrix N	
	Eigen::MatrixXd tmpMat = Eigen::MatrixXd::Zero(3,3);
	double A12, A20, A01;
	double traceM = 0.0;
	for(i=0; i<3; i++) traceM += M(i,i);

	tmpMat.diagonal() = Eigen::VectorXd::Constant(3, -traceM); //tmpMat.fill_diagonal(-traceM);
	tmpMat += (M + M.transpose());

	A12 = M(1,2) - M(2,1);
	A20 = M(2,0) - M(0,2);
	A01 = M(0,1) - M(1,0);

	N(0,0)=traceM; N(0,1)=A12; N(0,2)=A20; N(0,3)=A01;
	N(1,0)=A12;
	N(2,0)=A20;
	N(3,0)=A01;

	N.bottomRightCorner(3,3) = tmpMat; //N.update(tmpMat,1,1);

	////find the eigenvector that belongs to the maximal 
	////eigenvalue of N, eigenvalues are sorted from smallest to largest
	//vnl_symmetric_eigensystem<double> eigenSystem(N);
	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es;
	es.compute(N);
	Eigen::MatrixXd V = es.eigenvectors();

	//std::stringstream ss;ss << V;
	//qDebug() << qPrintable(ss.str().c_str());

	//setRotationQuaternion(eigenSystem.V(0,3),eigenSystem.V(1,3),eigenSystem.V(2,3),eigenSystem.V(3,3), true);
	result = Eigen::Quaterniond( V(0,3),V(1,3),V(2,3),V(3,3) ).normalized();
}