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; }