У меня есть 3D-точка в мировых координатах, (-140, -500,0), где z - вектор вверх, x - глубина, а y - горизонталь. Теперь я хочу преобразовать эту точку в координаты камеры, которые я знаю. что мне нужно рассчитать матрицу вращения и сдвиг, у меня есть шаг и отклонение крена и положение камеры
Я хочу знать, правильно ли я вычисляю точку в координатах камеры
//ax, ay and az are the position of the point in the real world
//cx, cy and cz are the position of the camera
//67.362312316894531 is pitch
//89.7135009765625 is roll
//0.033716827630996704 is yaw
double x = ax - cx;
double y = ay -cy;
double z = az - cz;
double cosx = cos(67.362312316894531);
double sinx = sin(67.362312316894531);
double cosy = cos(89.7135009765625);
double siny = sin(89.7135009765625);
double cosz = cos(0.033716827630996704);
double sinz = sin(0.033716827630996704);
dx = cosy * (sinz * y + cosz * x) - siny * z;
dy = sinx * (cosy * z + siny * (sinz * y + cosz * x)) + cosx * (cosz * y - sinz * x);
dz = cosx * (cosy * z + siny * (sinz * y + cosz * x)) - sinx * (cosz * y - sinz * x);
Я знаю, что Другой метод состоит в том, чтобы вычислить матрицу вращения
//where yp is pitch, thet is roll and k is yaw
double rotxm[9] = { 1,0,0,0,cos(yp),-sin(yp),0,sin(yp),cos(yp) };
double rotym[9] = { cos(thet),0,sin(thet),0,1,0,-sin(thet),0,cos(thet) };
double rotzm[9] = { cos(k),-sin(k),0,sin(k),cos(k),0,0,0,1};
cv::Mat rotx = Mat{ 3,3,CV_64F,rotxm };
cv::Mat roty = Mat{ 3,3,CV_64F,rotym };
cv::Mat rotz = Mat{ 3,3,CV_64F,rotzm };
cv::Mat rotationm = rotz * roty * rotx;
Мой вопрос, правильны ли эти два метода? или хотя бы один из них .. как я могу убедиться в этом