Eigen使用

1.赋值操作

using Vec3 = Eigen::Matrix<double, 3, 1>;
Vec3 gyro_noise_std;
gyro_noise_std.setConstant(0.000282);
//将vec中的所有值都设为相同数值

2.四元数与矩阵转换

//四元数转旋转矩阵
Eigen::Matrix4d T_W_BC = Eigen::Matrix4d::Identity();
Eigen::Quaterniond q(q_w,q_x,q_y,q_z);
q.normalize();
T_W_BC.block<3,3>(0,0) = q.toRotationMatrix();
T_W_BC.block<3,1>(0,3) << pos_x, pos_y, pos_z;
//旋转矩阵转四元数
Eigen::Matrix3d R_mat = Eigen::Matrix3d::Identity();
Eigen::Quaterniond q(R_mat);

3.Sophus与Eigen转换

SE3d T_w_i;
Eigen::Vec3d p = T_w_i.translation();
Eigen::Quaterniond q(T_w_i.so3().matrix());
savetwb << double(last_state_t_ns) * 1e-9 << " "
            << p.x() << " " << p.y() << " " << p.z() << " "
            << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << " "
            << std::endl;

4.Eigen旋转矩阵与轴角转换

Eigen::Matrix3d axis_to_rot_mat(const Eigen::Vector3d& angle_axis) {
  Eigen::Matrix3d rot_mat;
  const double angle = angle_axis.norm();
  if (angle <= 1e-5) {
      rot_mat = Eigen::Matrix3d::Identity();
  }
  else {
      const Eigen::Vector3d axis = angle_axis / angle;
      rot_mat = Eigen::AngleAxisd(angle, axis).toRotationMatrix();
  }
  return rot_mat;
}

//example
const Eigen::Matrix3d rot_gt = to_rot_mat(10.0 * M_PI / 180 * Eigen::Vector3d{1.0, 0, 0}.normalized());

Eigen::Vector3d mat_to_angle_axis(const Eigen::Matrix3d& rot_mat) {
  const Eigen::AngleAxisd angle_axis(rot_mat);
  return angle_axis.axis() * angle_axis.angle();
}