使用eigen时要确保数据类型一致 否则会报错:
Eigen: YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUME
Eigen::Quaterniond quaternion(imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z);
Eigen::Matrix3f rotation_matrix = quaternion.toRotationMatrix();
法一:将第一行的double类型改为float
Eigen::Quaternionf quaternion(imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z);
Eigen::Matrix3f rotation_matrix = quaternion.toRotationMatrix();
法二:通过.cast()改为相应类型
Eigen::Quaternionf quaternion(imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z);
Eigen::Matrix3f rotation_matrix = quaternion.toRotationMatrix().cast<float>();
pose 里是 Rigid2d double
类型 需要转换
const Eigen::Vector2f transed_p = pose.cast<float>() * point.position.head<2>();