刚体三维空间变换常见函数

常见函数

四元数转欧拉角

def quart_to_rpy(x, y, z, w, deg=False):
    roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
    pitch = math.asin(2 * (w * y - x * z))
    yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (z * z + y * y))
    rpy = (roll, pitch, yaw)
    if deg:
        rpy = np.rad2deg(rpy)
        return rpy
    return rpy

空间旋转

void TransformPointsFromOdom2Imu(
        const drive::common::geometry::Point& odom_point,
        drive::common::geometry::Point& imu_point,
        const drive::common::pose::Pose3D& imu_pose) {
    Eigen::Matrix<double, 3, 1> translation{
            imu_pose.position().x(), imu_pose.position().y(), imu_pose.position().z()};
    Eigen::Quaternion<double> orientation{imu_pose.orientation().qx(),
                                          imu_pose.orientation().qy(),
                                          imu_pose.orientation().qz(),
                                          imu_pose.orientation().qw()};
    logger.debug(5, "translation: %%, orientation:%%", translation, orientation);

    Eigen::Matrix<double, 4, 4> transform_matrix = Eigen::Matrix<double, 4, 4>::Identity();
    Eigen::Matrix<double, 3, 3> R = orientation.toRotationMatrix();
    transform_matrix.block(0, 0, 3, 3) = R;
    transform_matrix.block(0, 3, 3, 1) = translation;

    Eigen::Matrix<double, 4, 1> p{odom_point.x(), odom_point.y(), odom_point.z(), 1};
    Eigen::Matrix<double, 4, 1> tmp_imu_point = transform_matrix.inverse() * p;

    imu_point.set_x(tmp_imu_point(0, 0));
    imu_point.set_y(tmp_imu_point(1, 0));
    imu_point.set_z(tmp_imu_point(2, 0));
}