最近使用vins-fusion做外参在线标定,在EuRoc多个序列上运行可以计算得到多个外参,想要求其平均,因此涉及到旋转矩阵求平均的问题。
参考知乎回答:获得多个旋转矩阵,如何获得平均旋转? - fly qq的回答 - 知乎 了解到可以通过构建最小二乘问题,最小化解与多个旋转的距离 进行求解。
最小二乘问题如下:
R ˉ = arg min R ∈ S O ( 3 ) ∑ i = 1 n d ( R , R i ) 2 \bar{R}=\arg \min _{R \in S O(3)} \quad \sum_{i=1}^n d\left(R, R_i\right)^2 Rˉ=argR∈SO(3)mini=1∑nd(R,Ri)2
其中两个 S O ( 3 ) SO(3) SO(3) 元素之间的距离定义为:
d ( R i , R j ) = ∥ log ( R i T R j ) ∥ d\left(R_i, R_j\right)=\left\|\log \left(R_i^T R_j\right)\right\| d(Ri,Rj)= log(RiTRj)
这里使用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
准确求出结果。