#include "AbsoluteOrientation.h" void AbsoluteOrientation::compute( Eigen::Vector3d Xa,Eigen::Vector3d Ya,Eigen::Vector3d Za, Eigen::Vector3d Xb,Eigen::Vector3d Yb,Eigen::Vector3d Zb, Eigen::Quaterniond &result ) { std::vector A,B; A.push_back(Xa); A.push_back(Ya); A.push_back(Za); B.push_back(Xb); B.push_back(Yb); B.push_back(Zb); compute(A,B,result); } void AbsoluteOrientation::compute( std::vector &left, std::vector &right, Eigen::Quaterniond &result ) { int i, pairNum = left.size(); Eigen::MatrixXd muLmuR = Eigen::MatrixXd::Zero(3,3), M = Eigen::MatrixXd::Zero(3,3), curMat = Eigen::MatrixXd::Zero(3,3), N = Eigen::MatrixXd::Zero(4,4); Eigen::Vector3d meanFirst(0,0,0), meanSecond(0,0,0); //assume points set to zero by constructor //compute the mean of both point sets for (i=0; i eigenSystem(N); Eigen::SelfAdjointEigenSolver es; es.compute(N); Eigen::MatrixXd V = es.eigenvectors(); //std::stringstream ss;ss << V; //qDebug() << qPrintable(ss.str().c_str()); //setRotationQuaternion(eigenSystem.V(0,3),eigenSystem.V(1,3),eigenSystem.V(2,3),eigenSystem.V(3,3), true); result = Eigen::Quaterniond( V(0,3),V(1,3),V(2,3),V(3,3) ).normalized(); } void AbsoluteOrientation::minOnT( std::vector &frameA, std::vector &frameB, Eigen::Quaterniond &result ) { // Find minimum rotation for T result = Eigen::Quaterniond::FromTwoVectors( frameA[2], frameB[2] ); // Apply to frame A for(int i = 0;i < (int)frameA.size(); i++) frameA[i] = result * frameA[i]; std::vector left, right; for(int i = 0;i < (int)frameA.size(); i++) { left.push_back( frameA[i] ); right.push_back( frameB[i] ); } Eigen::Quaterniond rot2; AbsoluteOrientation::compute(left, right, rot2); /*result = rot2result * rot2*/; }