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