I have a view matrix of my camera (as 4x4 matrix) and I'd like to extract rotation and translation vectors from it, and back (from rot and transl vectors to view matrix).
I have found some forumals for doing this but they does not work.
How can I do that?
For an augmented reality application I compute the view matrix from the distortion parameters and solvePnP. The method works: I find some points in the image plane (by clicking them) and, knowing the points in the real world, I can estimate the camera position.
The clicking procedure has some errors (click are not exact), so I'd like to refine the camera position by manually modify the translation and rotation vectors.
So I'm looking for a way to have vectors from matrix, and then go back, from vectors to matrix.
What I'm doing till now is:
void matrix2vector(cv::Mat n) { // n is the pose matrix
this->position[0] = n.at<float>(3);
this->position[1] = n.at<float>(7);
this->position[2] = n.at<float>(11);
float rx, ry, rz;
rx = atan2(n.at<float>(9), n.at<float>(10));
ry = atan2(-n.at<float>(8),
sqrt( n.at<float>(9)*n.at<float>(9) + n.at<float>(10)*n.at<float>(10) ));
rz = atan2(n.at<float>(4), n.at<float>(0));
this->angle[0] = rx * RAD2DEG;
this->angle[1] = ry * RAD2DEG;
this->angle[2] = rz * RAD2DEG;
}
void vector2matrix() {
_viewMatrix = cv::Mat::eye(4, 4, CV_32F);
rotateX(_viewMatrix, _viewMatrix, -angle[0]);
rotateY(_viewMatrix, _viewMatrix, -angle[1]);
rotateZ(_viewMatrix, _viewMatrix, -angle[2]);
_viewMatrix.at<float>(3) = position[0];
_viewMatrix.at<float>(7) = position[1];
_viewMatrix.at<float>(11) = position[2];
}
where the rotations methods are:
inline
void rotateX(cv::Mat &src, cv::Mat &dst, float angleDegree) {
dst = src.clone();
float c = cosf(angleDegree * DEG2RAD);
float s = sinf(angleDegree * DEG2RAD);
float m4 = src.at<float>(4),
m5 = src.at<float>(5),
m6 = src.at<float>(6),
m7 = src.at<float>(7),
m8 = src.at<float>(8),
m9 = src.at<float>(9),
m10= src.at<float>(10),
m11= src.at<float>(11);
dst.at<float>(4) = m4 * c + m8 *-s;
dst.at<float>(5) = m5 * c + m9 *-s;
dst.at<float>(6) = m6 * c + m10*-s;
dst.at<float>(7) = m7 * c + m11*-s;
dst.at<float>(8) = m4 * s + m8 * c;
dst.at<float>(9) = m5 * s + m9 * c;
dst.at<float>(10)= m6 * s + m10* c;
dst.at<float>(11)= m7 * s + m11* c;
}
inline
void rotateY(cv::Mat &src, cv::Mat &dst, float angleDegree)
{
float c = cosf(angleDegree * DEG2RAD);
float s = sinf(angleDegree * DEG2RAD);
float m0 = src.at<float>(0),
m1 = src.at<float>(1),
m2 = src.at<float>(2),
m3 = src.at<float>(3),
m8 = src.at<float>(8),
m9 = src.at<float>(9),
m10= src.at<float>(10),
m11= src.at<float>(11);
dst.at<float>(0) = m0 * c + m8 * s;
dst.at<float>(1) = m1 * c + m9 * s;
dst.at<float>(2) = m2 * c + m10* s;
dst.at<float>(3) = m3 * c + m11* s;
dst.at<float>(8) = m0 *-s + m8 * c;
dst.at<float>(9) = m1 *-s + m9 * c;
dst.at<float>(10)= m2 *-s + m10* c;
dst.at<float>(11)= m3 *-s + m11* c;
}
inline
void rotateZ(cv::Mat &src, cv::Mat &dst, float angleDegree)
{
float c = cosf(angleDegree * DEG2RAD);
float s = sinf(angleDegree * DEG2RAD);
float m0 = src.at<float>(0),
m1 = src.at<float>(1),
m2 = src.at<float>(2),
m3 = src.at<float>(3),
m4 = src.at<float>(4),
m5 = src.at<float>(5),
m6 = src.at<float>(6),
m7 = src.at<float>(7);
dst.at<float>(0) = m0 * c + m4 *-s;
dst.at<float>(1) = m1 * c + m5 *-s;
dst.at<float>(2) = m2 * c + m6 *-s;
dst.at<float>(3) = m3 * c + m7 *-s;
dst.at<float>(4) = m0 * s + m4 * c;
dst.at<float>(5) = m1 * s + m5 * c;
dst.at<float>(6) = m2 * s + m6 * c;
dst.at<float>(7) = m3 * s + m7 * c;
}
The answer is dependent upon whether you're using column or row vectors to transform vectors by matrices. Assuming you have a matrix M and a vector v and you transform it by v' = Mv, you're using column vectors. If instead you have v' = vM you're using row vectors.
To find the translation, simply check where the vector [0 0 0 1] ends up. If you're using column vectors, you'll find that the last column of your matrix is where the translation lies. The first three elements of that last column will be the translation vector (x,y,z). Similarly, in a row vector you'll find that the last row of your matrix is where the translation lies. The first three elements of the last row of your matrix will be the translation vector (x,y,z).
For rotation, try multiplying your matrix by the vector [1 0 0 0]. This will tell you where the "x axis" maps to. Similarly for [0 1 0 0] (y axis) and [0 0 1 0] (z axis). Whether you're using column or row vectors, the relevant entries are the 3x3 sub-matrix in the top left of your 4x4 matrix. The only thing that changes between row and column vectors is whether or not the mapped axis is a column vector or row vector in the submatrix.
Hope that helps.