Exemplos de eigen_vector em C++ (Cpp)

Exemplo n.º 1

Arquivo: Element.C Projeto: Jieun2/moose

void
Element::polarDecompositionEigen( const ColumnMajorMatrix & Fhat, ColumnMajorMatrix & Rhat, SymmTensor & strain_increment )
{
  const int ND = 3;

  ColumnMajorMatrix eigen_value(ND,1), eigen_vector(ND,ND);
  ColumnMajorMatrix invUhat(ND,ND), logVhat(ND,ND);
  ColumnMajorMatrix n1(ND,1), n2(ND,1), n3(ND,1), N1(ND,1), N2(ND,1), N3(ND,1);

  ColumnMajorMatrix Chat = Fhat.transpose() * Fhat;

  Chat.eigen(eigen_value,eigen_vector);

  for(int i = 0; i < ND; i++)
  {
    N1(i) = eigen_vector(i,0);
    N2(i) = eigen_vector(i,1);
    N3(i) = eigen_vector(i,2);
  }

  const Real lamda1 = std::sqrt(eigen_value(0));
  const Real lamda2 = std::sqrt(eigen_value(1));
  const Real lamda3 = std::sqrt(eigen_value(2));


  const Real log1 = std::log(lamda1);
  const Real log2 = std::log(lamda2);
  const Real log3 = std::log(lamda3);

  ColumnMajorMatrix Uhat = N1 * N1.transpose() * lamda1 +  N2 * N2.transpose() * lamda2 +  N3 * N3.transpose() * lamda3;

  invertMatrix(Uhat,invUhat);

  Rhat = Fhat * invUhat;

  strain_increment = N1 * N1.transpose() * log1 +  N2 * N2.transpose() * log2 +  N3 * N3.transpose() * log3;
}

Exemplo n.º 2

Arquivo: marker_calibration.cpp Projeto: jondo/omnirob_robin

geometry_msgs::Quaternion calc_quaternion_average( std::vector<geometry_msgs::Pose> pose_vector ){
	
	Eigen::Matrix4f averaging_matrix;
	Eigen::Vector4f quaternion;
	
	averaging_matrix.setZero();
	for( unsigned int sample_ii=0; sample_ii<pose_vector.size(); sample_ii++){
		quaternion(0) = pose_vector[sample_ii].orientation.x;
		quaternion(1) = pose_vector[sample_ii].orientation.y;
		quaternion(2) = pose_vector[sample_ii].orientation.z;
		quaternion(3) = pose_vector[sample_ii].orientation.w;
		averaging_matrix =  averaging_matrix + quaternion*quaternion.transpose();
	  }// for all samples
	 
	 Eigen::EigenSolver<Eigen::Matrix4f> ev_solver;
	 ev_solver.compute( averaging_matrix);
	 Eigen::Vector4cf eigen_values = ev_solver.eigenvalues();
	 float max_ev=eigen_values(0).real();
	 unsigned int max_ii = 0;
	 for( unsigned int ev_ii=1; ev_ii<4; ev_ii++){
		 if( eigen_values(ev_ii).real()>max_ev ){
			 max_ev = eigen_values(ev_ii).real();
			 max_ii=ev_ii;
		 }
	 }
	 
	 Eigen::Vector4f eigen_vector = ev_solver.eigenvectors().col(max_ii).real();
	 eigen_vector.normalize();
	  
	 geometry_msgs::Quaternion quaternion_msg;
	 quaternion_msg.x = eigen_vector(0);
	 quaternion_msg.y = eigen_vector(1);
	 quaternion_msg.z = eigen_vector(2);
	 quaternion_msg.w = eigen_vector(3);
	 return quaternion_msg;
	 
}