void Node::setEulerAngles(const float& x, const float& y, const float& z)
{
if (mUseEulerLimits)
{
float ex = x;
float ey = y;
float ez = z;
constrain(ex, mEulerLimitsX);
constrain(ey, mEulerLimitsY);
constrain(ez, mEulerLimitsZ);
ci::Matrix33<float> X(1, 0, 0, 0, cos(ex), sin(ex), 0, -1.*sin(ex), cos(ex));
ci::Matrix33<float> Y(cos(ey), 0, -1.*sin(ey), 0, 1, 0, sin(ey), 0, cos(ey));
ci::Matrix33<float> Z(cos(ez), sin(ez), 0, -1.*sin(ez), cos(ez), 0, 0, 0, 1);
ci::Matrix33<float> R = Z * Y * X;
ci::Matrix33<float> qMat(R);
setOrientation(qMat);
}
else
{
ci::Matrix33<float> X(1, 0, 0, 0, cos(x), sin(x), 0, -1.*sin(x), cos(x));
ci::Matrix33<float> Y(cos(y), 0, -1.*sin(y), 0, 1, 0, sin(y), 0, cos(y));
ci::Matrix33<float> Z(cos(z), sin(z), 0, -1.*sin(z), cos(z), 0, 0, 0, 1);
ci::Matrix33<float> R = Z * Y * X;
ci::Matrix33<float> qMat(R);
setOrientation(qMat);
}
}
// This is the model equation for the timeframe RANSAC
// B.K.P. Horn's closed form Absolute Orientation method (1987 paper)
// The convention used here is: right = (1/scale) * RMat * left + TMat
int Photogrammetry::absoluteOrientation(vector<cv::Point3d> & left, vector<cv::Point3d> & right, cv::Mat & RMat, cv::Mat & TMat, double & scale) {
//check if both vectors have the same number of size
if (left.size() != right.size()) {
cerr << "Sizes don't match" << endl;
return -1;
}
//compute the mean of the left and right set of points
cv::Point3d leftmean, rightmean;
leftmean.x = 0;
leftmean.y = 0;
leftmean.z = 0;
rightmean.x = 0;
rightmean.y = 0;
rightmean.z = 0;
for (int i = 0; i < left.size(); i++) {
leftmean.x += left[i].x;
leftmean.y += left[i].y;
leftmean.z += left[i].z;
rightmean.x += right[i].x;
rightmean.y += right[i].y;
rightmean.z += right[i].z;
}
leftmean.x /= left.size();
leftmean.y /= left.size();
leftmean.z /= left.size();
rightmean.x /= right.size();
rightmean.y /= right.size();
rightmean.z /= right.size();
cv::Mat leftmeanMat(3,1,CV_64F);
cv::Mat rightmeanMat(3,1,CV_64F);
leftmeanMat.at<double>(0,0) = leftmean.x;
leftmeanMat.at<double>(0,1) = leftmean.y;
leftmeanMat.at<double>(0,2) = leftmean.z;
rightmeanMat.at<double>(0,0) = rightmean.x;
rightmeanMat.at<double>(0,1) = rightmean.y;
rightmeanMat.at<double>(0,2) = rightmean.z;
//normalize all points
for (int i = 0; i < left.size(); i++) {
left[i].x -= leftmean.x;
left[i].y -= leftmean.y;
left[i].z -= leftmean.z;
right[i].x -= rightmean.x;
right[i].y -= rightmean.y;
right[i].z -= rightmean.z;
}
//compute scale (use the symmetrical solution)
double Sl = 0;
double Sr = 0;
// this is the symmetrical version of the scale !
for (int i = 0; i < left.size(); i++) {
Sl += left[i].x*left[i].x + left[i].y*left[i].y + left[i].z*left[i].z;
Sr += right[i].x*right[i].x + right[i].y*right[i].y + right[i].z*right[i].z;
}
scale = sqrt(Sr/Sl);
// cout << "Scale: " << scale << endl;
//create M matrix
double M[3][3];// = {0.0};
/*
// I believe this is wrong, since not summing over all left right elements, just for the last element ! KM Nov 21
for (int i = 0; i < left.size(); i++) {
M[0][0] = left[i].x*right[i].x;
M[0][1] = left[i].x*right[i].y;
M[0][2] = left[i].x*right[i].z;
M[1][0] = left[i].y*right[i].x;
M[1][1] = left[i].y*right[i].y;
M[1][2] = left[i].y*right[i].z;
M[2][0] = left[i].z*right[i].x;
M[2][1] = left[i].z*right[i].y;
M[2][2] = left[i].z*right[i].z;
}
*/
M[0][0] = 0;
M[0][1] = 0;
M[0][2] = 0;
M[1][0] = 0;
M[1][1] = 0;
M[1][2] = 0;
M[2][0] = 0;
M[2][1] = 0;
M[2][2] = 0;
for (int i = 0; i < left.size(); i++)
{
M[0][0] += left[i].x*right[i].x;
M[0][1] += left[i].x*right[i].y;
M[0][2] += left[i].x*right[i].z;
M[1][0] += left[i].y*right[i].x;
M[1][1] += left[i].y*right[i].y;
M[1][2] += left[i].y*right[i].z;
M[2][0] += left[i].z*right[i].x;
M[2][1] += left[i].z*right[i].y;
M[2][2] += left[i].z*right[i].z;
}
//create N matrix
cv::Mat N = cv::Mat::zeros(4,4,CV_64F);
N.at<double>(0,0) = M[0][0] + M[1][1] + M[2][2];
N.at<double>(0,1) = M[1][2] - M[2][1];
N.at<double>(0,2) = M[2][0] - M[0][2];
N.at<double>(0,3) = M[0][1] - M[1][0];
N.at<double>(1,0) = M[1][2] - M[2][1];
N.at<double>(1,1) = M[0][0] - M[1][1] - M[2][2];
N.at<double>(1,2) = M[0][1] + M[1][0];
N.at<double>(1,3) = M[2][0] + M[0][2];
N.at<double>(2,0) = M[2][0] - M[0][2];
N.at<double>(2,1) = M[0][1] + M[1][0];
N.at<double>(2,2) = -M[0][0] + M[1][1] - M[2][2];
N.at<double>(2,3) = M[1][2] + M[2][1];
N.at<double>(3,0) = M[0][1] - M[1][0];
N.at<double>(3,1) = M[2][0] + M[0][2];
N.at<double>(3,2) = M[1][2] + M[2][1];
N.at<double>(3,3) = -M[0][0] - M[1][1] + M[2][2];
// cout << "N: " << N << endl;
//compute eigenvalues
cv::Mat eigenvalues(1,4,CV_64FC1);
cv::Mat eigenvectors(4,4,CV_64FC1);
// cout << "eigenvalues: \n" << eigenvalues << endl;
if (!cv::eigen(N, eigenvalues, eigenvectors)) {
cerr << "eigen failed" << endl;
return -1;
}
// cout << "Eigenvalues:\n" << eigenvalues << endl;
// cout << "Eigenvectors:\n" << eigenvectors << endl;
//compute quaterion as maximum eigenvector
double q[4];
q[0] = eigenvectors.at<double>(0,0);
q[1] = eigenvectors.at<double>(0,1);
q[2] = eigenvectors.at<double>(0,2);
q[3] = eigenvectors.at<double>(0,3);
/* // I believe this changed with the openCV implementation, eigenvectors are stored in row-order !
q[0] = eigenvectors.at<double>(0,0);
q[1] = eigenvectors.at<double>(1,0);
q[2] = eigenvectors.at<double>(2,0);
q[3] = eigenvectors.at<double>(3,0);
*/
double absOfEigVec = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] /= absOfEigVec;
q[1] /= absOfEigVec;
q[2] /= absOfEigVec;
q[3] /= absOfEigVec;
cv::Mat qMat(4,1,CV_64F,q);
// cout << "q: " << qMat << endl;
//compute Rotation matrix
RMat.at<double>(0,0) = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];
RMat.at<double>(0,1) = 2*(q[1]*q[2] - q[0]*q[3]);
RMat.at<double>(0,2) = 2*(q[1]*q[3] + q[0]*q[2]);
RMat.at<double>(1,0) = 2*(q[2]*q[1] + q[0]*q[3]);
RMat.at<double>(1,1) = q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3];
RMat.at<double>(1,2) = 2*(q[2]*q[3] - q[0]*q[1]);
RMat.at<double>(2,0) = 2*(q[3]*q[1] - q[0]*q[2]);
RMat.at<double>(2,1) = 2*(q[2]*q[3] + q[0]*q[1]);
RMat.at<double>(2,2) = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
// cout <<"R:\n" << RMat << endl;
// cout << "Det: " << determinant(RMat) << endl;
//find translation
cv::Mat tempMat(3,1,CV_64F);
//gemm(RMat, leftmeanMat, -1.0, rightmeanMat, 1.0, TMat); // enforcing scale of 1, since same scales in both frames
// The convention used here is: right = (1/scale) * RMat * left + TMat
TMat = -(1/scale) * RMat*leftmeanMat + rightmeanMat;
// gemm(RMat, leftmeanMat, -1.0 * scale, rightmeanMat, 1.0, TMat);
// cout << "Translation: " << TMat << endl;
return 0;
}
