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