eigen求四元数平均数

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

}