#include<Eigen/Core>
#include<iostream>
#include<Eigen/Geometry>
#include<Eigen/Dense>
using namespace Eigen;
void Calcuc(std::vector<Eigen::Quaterniond> qs)
{
Eigen::Matrix4d M;
M.setZero();
for (size_t i = 0; i < qs.size(); i++)
{
Eigen::Vector4d q(qs[i].x(), qs[i].y(), qs[i].z(), qs[i].w());
Eigen::Matrix4d Mi= q* q.transpose();
M += Mi;
}
std::cout << M << std::endl;
EigenSolver<Matrix4d> es(M);
MatrixXcd evecs = es.eigenvectors();//获取矩阵特征向量4*4，这里定义的MatrixXcd必须有c，表示获得的是complex复数矩阵
MatrixXcd evals = es.eigenvalues();//获取矩阵特征值 4*1
MatrixXd evalsReal;//注意这里定义的MatrixXd里没有c
evalsReal = evals.real();//获取特征值实数部分
MatrixXf::Index evalsMax;
evalsReal.rowwise().sum().maxCoeff(&evalsMax);//得到最大特征值的位置
Vector4d q;
q << evecs.real()(0, evalsMax), evecs.real()(1, evalsMax),evecs.real()(2, evalsMax), evecs.real()(3, evalsMax);
std::cout <<"final\n" << q << std::endl;
}
int main()
{
Eigen::AngleAxisd rotation_vector(acos(-1) / 4, Eigen::Vector3d(0, 0, 1));
Eigen::Quaterniond Q(rotation_vector);
Eigen::Quaterniond Q1(Eigen::AngleAxisd(acos(-1) / 6, Eigen::Vector3d(0, 0, 1)));
Eigen::Quaterniond Q2(Eigen::AngleAxisd(acos(-1) / 4, Eigen::Vector3d(0, 0, 1)));
Eigen::Quaterniond Q3(Eigen::AngleAxisd(acos(-1) / 4.5, Eigen::Vector3d(0, 0, 1)));
std::cout <<"123Q:" << Q3 << std::endl;
std::cout << "**********四元数值：x,y,z,w*************" << std::endl;
std::cout << Q.x() << std::endl;
std::cout << Q.y() << std::endl;
std::cout << Q.z() << std::endl;
std::cout << Q.w() << std::endl;
auto vc=Q.vec();
Calcuc({Q,Q1,Q2});
}
