cov是(3*3矩阵),正则化如下:
Eigen::Matrix3f cov;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> s(cov);
Eigen::Vector3f eig_val = s.eigenvalues().real();
Eigen::Matrix3f eig_vectors = s.eigenvectors().real();
eig_values(0) = std::max(eig_values(0), (float)0.001 * eig_values(1));
Eigen::Matrix3f regularized_eigs;
regularized_eigs << eig_values(0), 0, 0,
0, eig_values(1), 0
0, 0, eig_values(2);
Eigen::Matrix3f new_cov;
new_cov = eig_vectors * regularized_eigs * eig_vectors.inverse();