1、对称矩阵构建
template<typename T>
Eigen::Matrix<T, 3, 3> skew_sym_mat(const Eigen::Matrix<T, 3, 1> &v)
{Eigen::Matrix<T, 3, 3> skew_sym_mat;skew_sym_mat<<0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0;return skew_sym_mat;
}
2、角度变量转李群SE（3）
#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0template<typename T>
Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &&ang)
{T ang_norm = ang.norm();Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();if (ang_norm > 0.0000001){Eigen::Matrix<T, 3, 1> r_axis = ang / ang_norm;Eigen::Matrix<T, 3, 3> K;K << SKEW_SYM_MATRX(r_axis);/// Roderigous Tranformationreturn Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K;}else{return Eye3;}
}template<typename T, typename Ts>
Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &ang_vel, const Ts &dt)
{T ang_vel_norm = ang_vel.norm();Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();if (ang_vel_norm > 0.0000001){Eigen::Matrix<T, 3, 1> r_axis = ang_vel / ang_vel_norm;Eigen::Matrix<T, 3, 3> K;K << SKEW_SYM_MATRX(r_axis);T r_ang = ang_vel_norm * dt;/// Roderigous Tranformationreturn Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K;}else{return Eye3;}
}template<typename T>
Eigen::Matrix<T, 3, 3> Exp(const T &v1, const T &v2, const T &v3)
{T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3);Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();if (norm > 0.00001){T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm};Eigen::Matrix<T, 3, 3> K;K << SKEW_SYM_MATRX(r_ang);/// Roderigous Tranformationreturn Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K;}else{return Eye3;}
}
