使用Ceres求解多个旋转量的平均

最近使用vins-fusion做外参在线标定，在EuRoc多个序列上运行可以计算得到多个外参，想要求其平均，因此涉及到旋转矩阵求平均的问题。
参考知乎回答：获得多个旋转矩阵，如何获得平均旋转？ - fly qq的回答 - 知乎 了解到可以通过构建最小二乘问题，最小化解与多个旋转的距离 进行求解。
最小二乘问题如下：

$$\bar{R}=\arg \underset{R\in SO(3)}{\min }\sum _{i=1}^{n}d{\left(R,R_i\right)}^2$$

其中两个 $SO(3)$ 元素之间的距离定义为：

$$d\left(R_i,R_j\right)=\Vert \log \left(R_i^T{R}_j\right)\Vert$$

这里使用Ceres实现一下这种方法。

代码

#include <iostream>
#include "ceres/ceres.h"
#include <vector>
#include <float.h>
#include <eigen3/Eigen/Dense>

using namespace std;
using namespace Eigen;
using namespace ceres;

// 四元数对数映射
template <typename T>
Eigen::Matrix<T, 3, 1> log_Quaternion(const Eigen::Quaternion<T> &q){
    
    
    const T u_norm = ceres::hypot(q.x(), q.y(), q.z());
    const T theta = ceres::atan2(u_norm, q.w());
    
    Eigen::Matrix<T, 3, 1> ret = Eigen::Matrix<T, 3, 1>::Zero();
    if(ceres::fpclassify(u_norm) != FP_ZERO){
    
    
        ret(0) = theta * q.x() / u_norm;
        ret(1) = theta * q.y() / u_norm;
        ret(2) = theta * q.z() / u_norm;
    }
    
    return ret;
}

// 旋转残差
class RotationError{
    
    

public:
    RotationError(const Eigen::Matrix3d &R_): R0(R_.transpose()){
    
    }
    RotationError(const Eigen::Quaterniond &R_): R0(R_.inverse()){
    
    }

    template <typename T>
    bool operator()(const T* const R_, T* residual_) const {
    
    
        Eigen::Map<const Eigen::Quaternion<T>> R(R_);
        Eigen::Map<Eigen::Matrix<T, 3, 1>> residual(residual_);

        // Eigen::Quaternion<T> q_err = R0.cast<T>().inverse() * R;
        Eigen::Quaternion<T> q_err = R0.cast<T>() * R;

        residual = log_Quaternion(q_err);

        return true;
    }

private:
    Eigen::Quaterniond R0;

};

int main(int argc, char **argv){
    
    

    Eigen::Quaterniond R[6];
    R[0] = Eigen::AngleAxisd(M_PI_4, Vector3d(1,0,0));      // 绕x轴旋转
    R[1] = Eigen::AngleAxisd(-M_PI_4, Vector3d(1,0,0));
    R[2] = Eigen::AngleAxisd(M_PI_4, Vector3d(0,1,0));      // 绕y轴旋转
    R[3] = Eigen::AngleAxisd(-M_PI_4, Vector3d(0,1,0));
    R[4] = Eigen::AngleAxisd(M_PI_4, Vector3d(0,0,1));      // 绕z轴旋转
    R[5] = Eigen::AngleAxisd(-M_PI_4, Vector3d(0,0,1));

    // 最后的解应该要是单位矩阵（实单位四元数）
    
    for(int i = 0; i < 6; i++)
    {
    
    
        R[i].normalize();
    }

    Eigen::Quaterniond R0 = R[0];
    Eigen::Quaterniond R_init = R0;

    ceres::Problem problem;
    ceres::Manifold *quaternion_manifold = new ceres::EigenQuaternionManifold();

    for(int i = 0; i < 6; i++){
    
    
        ceres::CostFunction *cost_function = new ceres::AutoDiffCostFunction<RotationError, 3, 4>(new RotationError(R[i]));
        problem.AddResidualBlock(cost_function, nullptr, R0.coeffs().data());
        problem.SetManifold(R0.coeffs().data(), quaternion_manifold);
    }

    Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;
    Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    std::cout << "R : " << endl << R_init.coeffs().transpose() << " -> " << R0.coeffs().transpose() << endl;
    std::cout << "R : " << endl << R_init.toRotationMatrix() << "\n -> \n" << R0.toRotationMatrix() << endl;

    return 0;
}

输出

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  9.090954e-01    0.00e+00    1.34e+00   0.00e+00   0.00e+00  1.00e+04        0    2.31e-05    5.82e-05
   1  4.677188e-01    4.41e-01    2.41e-01   0.00e+00   8.94e-01  1.95e+04        1    2.10e-05    1.16e-04
   2  4.626610e-01    5.06e-03    1.64e-02   3.90e-02   1.07e+00  5.86e+04        1    5.96e-06    1.28e-04
   3  4.626378e-01    2.32e-05    1.11e-03   2.64e-03   1.07e+00  1.76e+05        1    4.05e-06    1.36e-04
R : 
0.382683        0        0  0.92388 -> -0.000191638   2.7746e-18  1.30678e-17            1
R : 
        1         0         0
        0  0.707107 -0.707107
        0  0.707107  0.707107
 -> 
           1 -2.61366e-17   5.5442e-18
 2.61345e-17            1  0.000383277
-5.55422e-18 -0.000383277            1

准确求出结果。

参考

[1] 获得多个旋转矩阵，如何获得平均旋转？ - fly qq的回答 - 知乎