C++ eigen::SelfAdjointEigenSolver类代码示例

  void DrawableTransformCovariance::updateCovarianceDrawList() {
    GLParameterTransformCovariance *covarianceParameter = dynamic_cast<GLParameterTransformCovariance*>(_parameter);
    glNewList(_covarianceDrawList, GL_COMPILE); 
    if(_covariance != Eigen::Matrix3f::Zero() && 
       covarianceParameter && 
       covarianceParameter->show() && 
       covarianceParameter->scale() > 0.0f) {
      float scale = covarianceParameter->scale();
      Eigen::Vector4f color = covarianceParameter->color();
      
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver;
      eigenSolver.computeDirect(_covariance, Eigen::ComputeEigenvectors);

      Eigen::Vector3f lambda = eigenSolver.eigenvalues();      
      Eigen::Isometry3f I = Eigen::Isometry3f::Identity();
      I.linear() = eigenSolver.eigenvectors();
      I.translation() = Eigen::Vector3f(_mean.x(), _mean.y(), _mean.z());
      
      float sx = sqrt(lambda[0]) * scale;
      float sy = sqrt(lambda[1]) * scale;
      float sz = sqrt(lambda[2]) * scale;
      
      glPushMatrix();
      glMultMatrixf(I.data());
      glColor4f(color[0], color[1], color[2], color[3]);
      glScalef(sx, sy, sz);
      glCallList(_sphereDrawList);
      glPopMatrix();	    
    }
    glEndList();
  }

  void DrawableUncertainty::updateCovarianceDrawList() {
    GLParameterUncertainty *uncertaintyParameter = dynamic_cast<GLParameterUncertainty*>(_parameter);
    glNewList(_covarianceDrawList, GL_COMPILE); 
    if(_covarianceDrawList &&
       _covariances && 
       uncertaintyParameter && 
       uncertaintyParameter->ellipsoidScale() > 0.0f) {
      uncertaintyParameter->applyGLParameter();
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver;
      float ellipsoidScale = uncertaintyParameter->ellipsoidScale();
      for(size_t i = 0; i < _covariances->size(); i += uncertaintyParameter->step()) {
	Gaussian3f &gaussian3f = _covariances->at(i);
	Eigen::Matrix3f covariance = gaussian3f.covarianceMatrix();
	Eigen::Vector3f mean = gaussian3f.mean();
	eigenSolver.computeDirect(covariance, Eigen::ComputeEigenvectors);
	Eigen::Vector3f eigenValues = eigenSolver.eigenvalues();      
	Eigen::Isometry3f I = Eigen::Isometry3f::Identity();
	I.linear() = eigenSolver.eigenvectors();
	I.translation() = mean;
	float sx = sqrt(eigenValues[0]) * ellipsoidScale;
	float sy = sqrt(eigenValues[1]) * ellipsoidScale;
	float sz = sqrt(eigenValues[2]) * ellipsoidScale;
	glPushMatrix();
	glMultMatrixf(I.data());	
	sx = sx;
	sy = sy;
	sz = sz;
	glScalef(sx, sy, sz);
	glCallList(_sphereDrawList);
	glPopMatrix();	    
      }   
    }
    glEndList();
  }

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

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
NurbsTools::pca (const vector_vec2d &data, Eigen::Vector2d &mean, Eigen::Matrix2d &eigenvectors,
                 Eigen::Vector2d &eigenvalues)
{
  if (data.empty ())
  {
    printf ("[NurbsTools::pca] Error, data is empty\n");
    abort ();
  }

  mean = computeMean (data);

  unsigned s = unsigned (data.size ());

  Eigen::MatrixXd Q (2, s);

  for (unsigned i = 0; i < s; i++)
    Q.col (i) << (data[i] - mean);

  Eigen::Matrix2d C = Q * Q.transpose ();

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver (C);
  if (eigensolver.info () != Success)
  {
    printf ("[NurbsTools::pca] Can not find eigenvalues.\n");
    abort ();
  }

  for (int i = 0; i < 2; ++i)
  {
    eigenvalues (i) = eigensolver.eigenvalues () (1 - i);
    eigenvectors.col (i) = eigensolver.eigenvectors ().col (1 - i);
  }
}

inline void calcNormalsEigen(Mat &depth_img, Mat &points, Mat &normals, int k=11, float max_dist=0.02, bool dist_rel_z=true) {

	if (normals.rows != depth_img.rows || normals.cols != depth_img.cols || normals.channels() != 3) {
		normals = cv::Mat::zeros(depth_img.rows, depth_img.cols, CV_32FC3);
	}
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver;
	const float bad_point = std::numeric_limits<float>::quiet_NaN ();

	for (int y = 0; y < depth_img.rows; ++y) {
		for (int x = 0; x < depth_img.cols; ++x) {

			Eigen::Vector3f p_q = points.at<Eigen::Vector3f>(y,x);
			// depth-nan handle: bad point
			if (depth_img.at<float>(y, x) == 0 || p_q(0) != p_q(0)){
				normals.at<Eigen::Vector3f>(y,x) = Eigen::Vector3f(bad_point, bad_point, bad_point);
				continue;
			}

			Eigen::Matrix3f A = Eigen::Matrix3f::Zero();
			std::vector<Eigen::Vector3f> p_j_list;
			Eigen::Vector3f _p = Eigen::Vector3f::Zero();
			float max_dist_rel = max_dist * ((dist_rel_z)? p_q[2]*1.5 : 1);
			
			for (int k_y = y-k/2; k_y <= y+k/2; ++k_y) {
				for (int k_x = x-k/2; k_x <= x+k/2; ++k_x) {

					if(k_y<0 || k_x<0 || k_y>=depth_img.rows || k_x >= depth_img.cols)
						continue;
					if (k_y == y && k_x == x)
						continue;
					if (depth_img.at<float>(k_y, k_x) == 0)
						continue;

					Eigen::Vector3f p_j = points.at<Eigen::Vector3f>(k_y,k_x);
					if( max_dist_rel <= 0 || ((p_q - p_j).norm() <= max_dist_rel) ) {
						p_j_list.push_back(p_j);
						_p += p_j;
					}
				}
			}


			_p /= p_j_list.size();
			double weight_sum = 0;
			for (int i = 0; i < p_j_list.size(); ++i) {
				double w = 1.0/(p_j_list[i] - _p).squaredNorm();
				A += w*((p_j_list[i] - _p)*((p_j_list[i] - _p).transpose()));
				weight_sum += w;
			}
			A /= weight_sum;
			solver.computeDirect(A);
			Eigen::Vector3f normal = solver.eigenvectors().col(0).normalized();
			// flip to viewpoint (0,0,0)
			if(normal(2) > 0)
				normal *= -1;
			normals.at<Eigen::Vector3f>(y,x) = normal;
		}
	}
}

/// 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
NurbsTools::pca (const vector_vec3d &data, ON_3dVector &mean, Eigen::Matrix3d &eigenvectors,
		 Eigen::Vector3d &eigenvalues)
{
  if (data.empty ())
  {
    printf ("[NurbsTools::pca] Error, data is empty\n");
    abort ();
  }

  mean = computeMean (data);

  unsigned s = data.size ();

  ON_Matrix Q(3, s);

  for (unsigned i = 0; i < s; i++) {
    Q[0][i] = data[i].x - mean.x;
    Q[1][i] = data[i].y - mean.y;
    Q[2][i] = data[i].z - mean.z;
  }

  ON_Matrix Qt = Q;
  Qt.Transpose();

  ON_Matrix oC;
  oC.Multiply(Q,Qt);

  Eigen::Matrix3d C(3,3);
  for (unsigned i = 0; i < 3; i++) {
      for (unsigned j = 0; j < 3; j++) {
	  C(i,j) = oC[i][j];
      }
  }

  Eigen::SelfAdjointEigenSolver < Eigen::Matrix3d > eigensolver (C);
  if (eigensolver.info () != Eigen::Success)
  {
    printf ("[NurbsTools::pca] Can not find eigenvalues.\n");
    abort ();
  }

  for (int i = 0; i < 3; ++i)
  {
    eigenvalues (i) = eigensolver.eigenvalues () (2 - i);
    if (i == 2)
      eigenvectors.col (2) = eigenvectors.col (0).cross (eigenvectors.col (1));
    else
      eigenvectors.col (i) = eigensolver.eigenvectors ().col (2 - i);
  }
}

OBB::OBB(Mesh::const_iterator begin, Mesh::const_iterator end)
{
	if (begin == end)
	{
		axes = -ZERO_SIZE * Matrix3f::Identity(); //make it inside out (i guess)
		origin = Vector3f::Zero();
		return;
	}

	Vector3f centerOfMass = centroid(begin, end);
	Matrix3f inertiaTensor = Matrix3f::Zero();

	auto addPt = [&](const Vector3f& pt, float mass)
	{
		Vector3f lpos = pt - centerOfMass;

		inertiaTensor(0, 0) += (lpos.y()*lpos.y() + lpos.z()*lpos.z()) * mass;
		inertiaTensor(1, 1) += (lpos.x()*lpos.x() + lpos.z()*lpos.z()) * mass;
		inertiaTensor(2, 2) += (lpos.x()*lpos.x() + lpos.y()*lpos.y()) * mass;
		inertiaTensor(1, 0) -= lpos.x()*lpos.y() * mass;
		inertiaTensor(2, 0) -= lpos.x()*lpos.z() * mass;
		inertiaTensor(2, 1) -= lpos.y()*lpos.z() * mass;
	};

	for (const auto& tri : make_range(begin, end))
	{
		float area = TriNormal(tri).norm() / 6.f;
		addPt(tri.col(0), area);
		addPt(tri.col(1), area);
		addPt(tri.col(2), area);
	}

	Eigen::SelfAdjointEigenSolver<Matrix3f> es;
	es.computeDirect(inertiaTensor);
	axes = es.eigenvectors();

	float maxflt = std::numeric_limits<float>::max();
	Eigen::Vector3f min{ maxflt, maxflt, maxflt };
	Eigen::Vector3f max = -min;

	for (const auto& tri : make_range(begin, end))
	{
		min = min.cwiseMin((axes.transpose() * tri).rowwise().minCoeff());
		max = max.cwiseMax((axes.transpose() * tri).rowwise().maxCoeff());
	}

    extent = (max - min).cwiseMax(ZERO_SIZE) / 2.f;
    origin = axes * (min + extent);
}

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

template<typename PointT> bool
pcl::PCA<PointT>::initCompute ()
{
    if(!Base::initCompute ())
    {
        PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] failed");
        return (false);
    }
    if(indices_->size () < 3)
    {
        PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] number of points < 3");
        return (false);
    }

    // Compute mean
    mean_ = Eigen::Vector4f::Zero ();
    compute3DCentroid (*input_, *indices_, mean_);
    // Compute demeanished cloud
    Eigen::MatrixXf cloud_demean;
    demeanPointCloud (*input_, *indices_, mean_, cloud_demean);
    assert (cloud_demean.cols () == int (indices_->size ()));
    // Compute the product cloud_demean * cloud_demean^T
    Eigen::Matrix3f alpha = static_cast<Eigen::Matrix3f> (cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ());

    // Compute eigen vectors and values
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> evd (alpha);
    // Organize eigenvectors and eigenvalues in ascendent order
    for (int i = 0; i < 3; ++i)
    {
        eigenvalues_[i] = evd.eigenvalues () [2-i];
        eigenvectors_.col (i) = evd.eigenvectors ().col (2-i);
    }
    // If not basis only then compute the coefficients

    if (!basis_only_)
        coefficients_ = eigenvectors_.transpose() * cloud_demean.topRows<3> ();
    compute_done_ = true;
    return (true);
}

IGL_INLINE void igl::polar_dec(
  const Eigen::PlainObjectBase<DerivedA> & A,
  Eigen::PlainObjectBase<DerivedR> & R,
  Eigen::PlainObjectBase<DerivedT> & T,
  Eigen::PlainObjectBase<DerivedU> & U,
  Eigen::PlainObjectBase<DerivedS> & S,
  Eigen::PlainObjectBase<DerivedV> & V)
{
  using namespace std;
  using namespace Eigen;
  typedef typename DerivedA::Scalar Scalar;

  const Scalar th = std::sqrt(Eigen::NumTraits<Scalar>::dummy_precision());

  Eigen::SelfAdjointEigenSolver<DerivedA> eig;
  feclearexcept(FE_UNDERFLOW);
  eig.computeDirect(A.transpose()*A);
  if(fetestexcept(FE_UNDERFLOW) || eig.eigenvalues()(0)/eig.eigenvalues()(2)<th)
  {
    cout<<"resorting to svd 1..."<<endl;
    return polar_svd(A,R,T,U,S,V);
  }

  S = eig.eigenvalues().cwiseSqrt();

  V = eig.eigenvectors();
  U = A * V;
  R = U * S.asDiagonal().inverse() * V.transpose();
  T = V * S.asDiagonal() * V.transpose();

  S = S.reverse().eval();
  V = V.rowwise().reverse().eval();
  U = U.rowwise().reverse().eval() * S.asDiagonal().inverse();

  if(R.determinant() < 0)
  {
    // Annoyingly the .eval() is necessary
    auto W = V.eval();
    const auto & SVT = S.asDiagonal() * V.adjoint();
    W.col(V.cols()-1) *= -1.;
    R = U*W.transpose();
    T = W*SVT;
  }

  if(std::fabs(R.squaredNorm()-3.) > th)
  {
    cout<<"resorting to svd 2..."<<endl;
    return polar_svd(A,R,T,U,S,V);
  }
}

static void diagonalizeInertiaTensor( const Matrix3s& I, Matrix3s& R0, Vector3s& I0 )
{
  // Inertia tensor should by symmetric
  assert( ( I - I.transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
  // Inertia tensor should have positive determinant
  assert( I.determinant() > 0.0 );

  // Compute the eigenvectors and eigenvalues of the input matrix
  const Eigen::SelfAdjointEigenSolver<Matrix3s> es{ I };

  // Check for errors
  if( es.info() == Eigen::NumericalIssue )
  {
    std::cerr << "Warning, failed to compute eigenvalues of inertia tensor due to Eigen::NumericalIssue" << std::endl;
  }
  else if( es.info() == Eigen::NoConvergence )
  {
    std::cerr << "Warning, failed to compute eigenvalues of inertia tensor due to Eigen::NoConvergence" << std::endl;
  }
  else if( es.info() == Eigen::InvalidInput )
  {
    std::cerr << "Warning, failed to compute eigenvalues of inertia tensor due to Eigen::InvalidInput" << std::endl;
  }
  assert( es.info() == Eigen::Success );

  // Save the eigenvectors and eigenvalues
  I0 = es.eigenvalues();
  assert( ( I0.array() > 0.0 ).all() );
  assert( I0.x() <= I0.y() );
  assert( I0.y() <= I0.z() );
  R0 = es.eigenvectors();
  assert( fabs( fabs( R0.determinant() ) - 1.0 ) <= 1.0e-6 );

  // Ensure that we have an orientation preserving transform
  if( R0.determinant() < 0.0 )
  {
    R0.col( 0 ) *= -1.0;
  }
}