旋转求均值

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

前言

参考 zhihu.com/question/439497100/answer/1683258444，给定初始欧拉角，添加高斯噪声以求解均值，用欧拉角直接求平均，四元数求平均，李代数求平均，数值优化方法（用ceres自动求导，残差采用Frobenius norm - 根号3）求得平均值，代码如下：

提示：以下是本篇文章正文内容，下面案例可供参考

代码如下

#include <iostream>
#include <vector>
#include <random>
#include <ctime>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <ceres/ceres.h>
#include <yaml-cpp/yaml.h>

template <typename T>
using EigenContainer = std::vector<T, Eigen::aligned_allocator<T>>;


template<typename T>
Eigen::Matrix<T, 3, 1> RotMtoEuler(const Eigen::Matrix<T, 3, 3> &rot)
{
    //计算旋转矩阵元素 rot(0,0) 和 rot(1,0) 的平方和 开根号sy。
    T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0));
    //sy 小于一个很小的阈值 1e-6，则被认为是奇异情况。
    bool singular = sy < 1e-6;

    T x, y, z;
    if(!singular)//如果不是奇异情况
    {
        //用 atan2 函数计算欧拉角的 x、y 和 z 分量
        x = atan2(rot(2, 1), rot(2, 2));//计算 Roll 角（绕X轴旋转）。
        y = atan2(-rot(2, 0), sy);   //计算 Pitch 角（绕Y轴旋转）。
        z = atan2(rot(1, 0), rot(0, 0));  //计算 Yaw 角（绕Z轴旋转）。
    }
    else//如果是奇异情况，表示 Pitch 角接近 ±π/2，此时无法唯一地计算 Roll 和 Yaw 角
    {    
        // 处理奇异情况，此时 Roll 和 Yaw 角不能被唯一确定
        // 我们可以将 Roll 角设为0，而 Yaw 角设为 atan2(-rot(1, 2), rot(1, 1))
        x = atan2(-rot(1, 2), rot(1, 1));    
        y = atan2(-rot(2, 0), sy);    
        z = 0;
    }
    Eigen::Matrix<T, 3, 1> ang(x, y, z);
    return ang;
}

double NormalizeAngle(double angle) {
    while (angle > 180.0) {
        angle -= 360.0;
    }
    while (angle < -180.0) {
        angle += 360.0;
    }
    return angle;
}

// 定义距离度量函数
struct RotationDistanceCost {
    RotationDistanceCost(const Eigen::Matrix3d& Ri) : Ri_(Ri) {}

    template <typename T>
    bool operator()(const T* const Rj_ptr, T* residual) const {
        Eigen::Map<const Eigen::Matrix<T, 3, 3>> Rj(Rj_ptr);

        // Calculate the delta rotation matrix as the difference between Ri and Rj
        Eigen::Matrix<T, 3, 3> delta_R = Ri_.template cast<T>() * Rj.transpose();
        
        // Calculate the Frobenius norm of the delta rotation matrix
        T frobenius_norm = delta_R.norm();

        // Assign the norm as the residual
        residual[0] = frobenius_norm - static_cast<T>(std::sqrt(3.0));


        return true;
    }
private:
    const Eigen::Matrix3d Ri_;
};

//方法一实现
void EulerAnglesCalculateAverage(const EigenContainer<Eigen::Vector3d>& vec) {
    Eigen::Vector3d sum(0.0, 0.0, 0.0);
    if (vec.empty()) {
        // 添加判断边界
    }
    for (const Eigen::Vector3d& v : vec) {
        sum += v;
    }
    Eigen::Vector3d EulerAnglesAverage = sum / static_cast<double>(vec.size());
    EulerAnglesAverage = EulerAnglesAverage * 180.0 / M_PI;
    std::cout << "\n欧拉角求解均值为:\n" << EulerAnglesAverage.transpose() << std::endl;

    // 计算协方差矩阵
    Eigen::Matrix3d covariance_matrix = Eigen::Matrix3d::Zero();
    for (const Eigen::Vector3d& v : vec) {
        Eigen::Vector3d temp_v = v * 180 / M_PI;
        Eigen::Vector3d diff = temp_v - EulerAnglesAverage;
        //Eigen::Vector3d diff = v - initialEulerAngles;
        covariance_matrix += diff * diff.transpose();
    }
    covariance_matrix /= static_cast<double>(vec.size());
    std::cout << "欧拉角求解协方差矩阵:\n" << covariance_matrix << std::endl;
}

// 方法二实现
void QuaternionCalculateAverage(const EigenContainer<Eigen::Quaterniond>& vec) {
    Eigen::Quaterniond sum(0.0, 0.0, 0.0, 0.0);
    for (const Eigen::Quaterniond& q : vec) {
        sum.coeffs() += q.coeffs();
    }
    sum.coeffs() /= static_cast<double>(vec.size());
    sum.normalize();
    Eigen::Matrix3d temp1 = sum.toRotationMatrix();
    Eigen::Vector3d QuaternionAverage = RotMtoEuler(temp1);
    QuaternionAverage = QuaternionAverage * 180.0 / M_PI;

    std::cout << "\n四元数求解均值为:\n" << QuaternionAverage.transpose() << std::endl;
    // 计算协方差矩阵
    Eigen::Matrix3d covariance_matrix = Eigen::Matrix3d::Zero();
    for (const Eigen::Quaterniond& q : vec) {
        Eigen::Quaterniond diff_quaternion = q * sum.conjugate(); // 四元数差异
        Eigen::Vector3d diff_euler = RotMtoEuler(diff_quaternion.toRotationMatrix()); // 转换为欧拉角
        covariance_matrix += diff_euler * diff_euler.transpose();
    }
    covariance_matrix /= static_cast<double>(vec.size());
    std::cout << "四元数求解协方差矩阵:\n" << covariance_matrix << std::endl;
}        


// 方法三实现
void LieAlgebraCalculateAverage(const EigenContainer<Eigen::Vector3d>& vec) {
    Eigen::Vector3d sum(0.0, 0.0, 0.0);

    for (const Eigen::Vector3d& v : vec) {
        sum += v;
    }
    sum = sum / static_cast<double>(vec.size());

    Eigen::Matrix3d rotation_matrix_temp;
    Eigen::AngleAxisd rotation_vector_temp(sum.norm(), sum.normalized());
    rotation_matrix_temp = rotation_vector_temp.toRotationMatrix();
    Eigen::Vector3d LieAlgebraAverage = RotMtoEuler(rotation_matrix_temp);   
     
    LieAlgebraAverage = LieAlgebraAverage * 180.0 / M_PI;
    std::cout << "\n李代数求解均值为:\n" << LieAlgebraAverage.transpose()  <<  std::endl;

    // 计算协方差矩阵
    Eigen::Matrix3d covariance_matrix = Eigen::Matrix3d::Zero();
    for (const Eigen::Vector3d& v : vec) {
        Eigen::Vector3d diff = v - sum;
        covariance_matrix += diff * diff.transpose();
    }
    covariance_matrix /= static_cast<double>(vec.size());
    std::cout << "李代数求解协方差矩阵:\n" << covariance_matrix << std::endl;
}

// 方法四实现
void OptimizationCalculateAverage(const EigenContainer<Eigen::Matrix3d>& vec) {
    // 初始化Ceres Solver
    ceres::Problem problem;

    // 创建变量，表示平均旋转矩阵
    // 初始值可以使用第一个旋转矩阵
    double R_avg[9];
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            R_avg[i * 3 + j] = vec[0](i, j);
        }
    }

    problem.AddParameterBlock(R_avg, 9);

    // 添加每个旋转矩阵与平均值之间的距离代价
    for (const Eigen::Matrix3d& Ri : vec) {
        ceres::CostFunction* cost_function =
            new ceres::AutoDiffCostFunction<RotationDistanceCost, 1, 9>(new RotationDistanceCost(Ri));
        problem.AddResidualBlock(cost_function, nullptr, R_avg);
    }

    // 配置Ceres Solver选项
    ceres::Solver::Options options;
    options.max_num_iterations = 100000; // 设置最大迭代次数
    options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY; // 设置线性求解器类型
    options.minimizer_progress_to_stdout = false;
    options.num_threads = 7;
    ceres::Solver::Summary summary;

    // 解决优化问题
    ceres::Solve(options, &problem, &summary);

    // 输出优化结果
    Eigen::Matrix3d optimized_R;
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            optimized_R(i, j) = R_avg[i * 3 + j];
        }
    }
    Eigen::Vector3d EulerAngles = RotMtoEuler(optimized_R);     

    EulerAngles = EulerAngles * 180.0 / M_PI;
    std::cout << "\n数值优化后的欧拉角:\n" << EulerAngles.transpose()  <<  std::endl;

    // 计算协方差矩阵
    Eigen::Matrix3d covariance_matrix = Eigen::Matrix3d::Zero();
    for (const Eigen::Matrix3d& Ri : vec) {
        Eigen::Matrix3d diff_matrix = Ri * optimized_R.transpose();
        Eigen::Vector3d diff_euler = RotMtoEuler(diff_matrix);
        covariance_matrix += diff_euler * diff_euler.transpose();
    }
    covariance_matrix /= static_cast<double>(vec.size());

    // 打印协方差矩阵
    std::cout << "数值优化求解协方差矩阵：\n"  << covariance_matrix << std::endl;
}


int main() {
    double rolling, pitch, yaw, mean, standard;
    // 定义不同类型的Eigen对象的容器
    EigenContainer<Eigen::Matrix3d> Rotations;
    EigenContainer<Eigen::Quaterniond> Quaternions;
    EigenContainer<Eigen::Vector3d> EulerAngles;
    EigenContainer<Eigen::Vector3d> LieAlgebra;



    YAML::Node config = YAML::LoadFile("../config.yaml");

    rolling = config["rolling"].as<double>();
    rolling = NormalizeAngle(rolling);

    pitch = config["pitch"].as<double>();
    pitch = NormalizeAngle(pitch);

    yaw = config["yaw"].as<double>();
    yaw = NormalizeAngle(yaw);

    mean = config["mean"].as<double>();
    standard =  config["standard_deviation"].as<double>();

    int numSamples  = config["num"].as<int>();



    std::cout << "\n初始的欧拉角:\n" << rolling << " " << pitch << " " << yaw << std::endl;

    // 初始化欧拉角
    Eigen::Vector3d initialEulerAngles(rolling * M_PI / 180.0, pitch * M_PI / 180.0, yaw * M_PI / 180.0);
 
    std::default_random_engine generator(std::time(nullptr));
    std::normal_distribution<double> distribution(mean, standard); // 均值为0，标准差为0.1，

    std::cout << "\n噪声均值方差为:" << mean << ", " << standard << std::endl;
    std::cout << "\n生成" << numSamples << "个噪声数据" << std::endl;
    for (int i = 0; i < numSamples; ++i) {


        // 生成高斯噪声
        double noiseX = distribution(generator);
        double noiseY = distribution(generator);
        double noiseZ = distribution(generator);

        // 添加噪声到给定欧拉角
        Eigen::Vector3d noisyEulerAngles(
            initialEulerAngles.x() + noiseX * M_PI / 180,
            initialEulerAngles.y() + noiseY * M_PI / 180,
            initialEulerAngles.z() + noiseZ * M_PI / 180
        );
        EulerAngles.push_back(noisyEulerAngles);

        //欧拉角转四元数
        Eigen::Quaterniond noisyquaternion = Eigen::AngleAxisd(noisyEulerAngles.z(), Eigen::Vector3d::UnitZ())
                * Eigen::AngleAxisd(noisyEulerAngles.y(), Eigen::Vector3d::UnitY())
                * Eigen::AngleAxisd(noisyEulerAngles.x(), Eigen::Vector3d::UnitX());      
        noisyquaternion.normalize();
        noisyquaternion.normalize();
        Quaternions.push_back(noisyquaternion);
        
        //四元数->旋转矩阵
        Eigen::Matrix3d Q2rotationMatrix = noisyquaternion.toRotationMatrix();
        Rotations.push_back(Q2rotationMatrix);


        //四元数转李代数
        Eigen::AngleAxisd Q2RotationVector(noisyquaternion);
        Eigen::Vector3d lieAlgebra = Q2RotationVector.angle() * Q2RotationVector.axis();
        LieAlgebra.push_back(lieAlgebra);        
    }
  
    //方法
    EulerAnglesCalculateAverage(EulerAngles);

    // 方法二：四元数表示旋转，然后直接用四元数的四个参数相加后平均
    QuaternionCalculateAverage(Quaternions);

    // 方法三：旋转映射（Log）到其切空间（切空间是向量空间），在切空间做平均后，重新 Exp 到 SO(3) 空间。
    LieAlgebraCalculateAverage(LieAlgebra);

    // 方法四：用数值求解的方式计算
    OptimizationCalculateAverage(Rotations);

    return 0;

}

Cmakelists

cmake_minimum_required(VERSION 3.0)
project(caculateAverage)
set(CMAKE_CXX_STANDARD 11)
add_executable(caculateAverage caculate_average.cpp )
find_package(Eigen3 REQUIRED)
find_package(Ceres REQUIRED)
include_directories(
${EIGEN3_INCLUDE_DIR}
include)
target_link_libraries(caculateAverage ${CERES_LIBRARIES})