如何从右手系的旋转矩阵中计算出相机中心到物体的yaw和pitch，在c++中实现

在计算机视觉和图形学中，相机的旋转通常用旋转矩阵来表示。一个右手坐标系下的旋转矩阵可以通过其对角线上的元素来直接推导出相机相对于世界坐标系的俯仰角（pitch）和偏航角（yaw）。这里提供一个C++中的实现方法：

#include <iostream>
#include <cmath>

// 假设rotationMatrix是一个3x3的右手系旋转矩阵，其元素表示为：
// [r11 r12 r13]
// [r21 r22 r23]
// [r31 r32 r33]
void calculateYawPitchFromRotationMatrix(float rotationMatrix[3][3], float& yaw, float& pitch) {
    // yaw对应绕z轴旋转的角度，可以通过atan2(r12, r22)计算
    yaw = atan2(rotationMatrix[0][1], rotationMatrix[1][1]);

    // pitch对应绕x轴旋转的角度，可以通过atan2(-r31, sqrt(r32*r32 + r33*r33))计算
    // 注意这里需要取负号，因为右手系中俯仰角正方向和z轴正方向相反
    pitch = atan2(-rotationMatrix[2][0], sqrt(rotationMatrix[2][1]*rotationMatrix[2][1] + rotationMatrix[2][2]*rotationMatrix[2][2]));
}

int main() {
    float rotationMatrix[3][3] = {
        // 这里填充你的旋转矩阵数据
    };
    float yaw, pitch;
    calculateYawPitchFromRotationMatrix(rotationMatrix, yaw, pitch);

    std::cout << "Yaw angle: " << yaw << ", Pitch angle: " << pitch << std::endl;
    return 0;
}

在这段代码中，我们定义了一个`calculateYawPitchFromRotationMatrix`函数，它接受一个3x3的旋转矩阵和两个引用参数（yaw和pitch），然后计算出对应的偏航角和俯仰角。