// Find matrix rotation between two spaces cv::Mat RTFinderTrilateration::findMatrixRotation(const Triple data) { cv::Mat b(3, 3, CV_32FC1); cv::Mat r(3, 3, CV_32FC1); cv::Mat A(3, 3, CV_32FC1); for (int i = 0;i<3 ; i++) { A.at<float>(i,0) = data.getPoint3d(0,i).x; A.at<float>(i,1) = data.getPoint3d(0,i).y; A.at<float>(i,2) = data.getPoint3d(0,i).z; } for (int i = 0;i<3 ; i++) { b.at<float>(i,0) = data.getPoint3d(1,i).x - data.translation_vector.x; b.at<float>(i,1) = data.getPoint3d(1,i).y - data.translation_vector.y; b.at<float>(i,2) = data.getPoint3d(1,i).z - data.translation_vector.z; } for (int i = 0; i < 3; ++i) cv::solve(A, b.col(i), r.col(i), cv::DECOMP_SVD); return r; }