Step2:向量归一化
?
Step3:修正的罗德里格斯参数表示姿态变化
?
Step4:计算初始旋转向量P′cg
?...Step5:计算旋转向量Pcg
?
Step6:计算旋转矩阵Rcg
?
Step7:计算平移向量TcgTcg
?...(3, 1, CV_64FC1);
Mat Pcij(3, 1, CV_64FC1);
Mat tempA(3, 3, CV_64FC1);
Mat tempb(3, 1, CV_64FC1);
Mat...);
Mat Rcg(3, 3, CV_64FC1);
Mat eyeM = Mat::eye(3, 3, CV_64FC1);
Mat Tgij(3, 1, CV_64FC1);
Mat Tcij...(3, 1, CV_64FC1);
Mat tempAA(3, 3, CV_64FC1);
Mat tempbb(3, 1, CV_64FC1);
Mat AA;
Mat bb;
Mat pinAA