/** * Find 3D rotation and translation with homography (i.e. calibration boards position in the camera cooperate system ) * @param h**o: [Input] Estimated Homography * @param rot: [Output] Rotation result * @param trans: [Output] Translation result */ void AugmentationEnvironment::H2Rt(const cv::Mat_<double> intrisinc, const cv::Mat_<double>& h**o, cv::Mat_<double>& rot, cv::Mat_<double> &trans) { double coefH; cv::Mat_<double> invcam = intrisinc.inv(); rot = invcam*h**o; //scaling rotation matrix to right order = find the coefficient lost while calculating homography /** * [Cam]*[Rot]' = [H**o]*coefH * |fx 0 u0| * [Cam]' = | 0 fy v0| * | 0 0 1| * * |R11 R12 t1| * [Rot]' = |R21 R22 t2| * |R31 R32 t3| * * |H11 H12 H13| * [H**o]' = |H21 H22 H23| * |H31 H32 1| */ coefH = 2/(cv::norm(rot.col(0)) + cv::norm(rot.col(1))); rot = rot * coefH; trans = rot.col(2).clone(); //The their rotation vector in the rotation matrix is produced as the cross product of other two vectors cv::Mat tmp = rot.col(2); rot.col(0).cross(rot.col(1)).copyTo(tmp); //Add here to make rotation matrix a "real" rotation matrix in zhang's method /** * rot = UDVt * rot_real = UVt */ cv::Mat_<double> D,U,Vt; cv::SVD::compute(rot,D,U,Vt); rot = U*Vt; }
T hotelling1_t2_score( cv::Mat_<T> mean1, cv::Mat_<T> mean2, cv::Mat_<T> cov1, unsigned n1){ unsigned k=mean1.size[0]; cv::Mat_<T > mean_diff=mean1-mean2; T t2_score= mean_diff.dot(cov1.inv() * mean_diff)*n1; return t2_score;}