//------------------------------------------------------------------------------
vec OrbitalEquation::getSvdRho1(){
cx_mat X;
vec s;
cx_mat Y;
#if WITH_SPIN
svd_econ(X, s, Y, rho1);
#else
svd_econ(X, s, Y, invRho);
#endif
return s;
}
//------------------------------------------------------------------------------
void TimePropagation::renormalize(cx_mat &D)
{
// Re-normalization of C using SVD
cx_mat X;
vec s;
cx_mat Y;
svd_econ(X, s, Y, D);
D = X*Y.t();
}
arma_deprecated
inline
bool
svd_thin
(
Mat<typename T1::elem_type>& U,
Col<typename T1::pod_type >& S,
Mat<typename T1::elem_type>& V,
const Base<typename T1::elem_type,T1>& X,
const char mode = 'b',
const typename arma_blas_type_only<typename T1::elem_type>::result* junk = 0
)
{
arma_ignore(junk);
return svd_econ(U,S,V,X,mode);
}
arma::mat Mahalanobis::getDecomposition() {
mat Z;
mat U, V;
vec s;
svd_econ(U, s, V, M);
for(int i = 0; i < s.n_elem; ++i)
s(i) = sqrt(s(i));
mat matS = diagmat(s);
Z = U * matS;
return Z;
}
inline mat orth(const mat & A)
{
mat U,V;
vec s;
int m,n;
svd_econ(U,s,V,A);
m = A.n_rows; n = A.n_cols;
double tol = MAX(m,n) * max(s) * math::eps();
int r = 0;
for (int i=0; i<s.n_elem; i++)
{
if (s(i) > tol)
{
r++;
}
}
return U.cols(0, r-1);
};
/******************************************************************
* 函数功能：几何校正
*
* 待写：H_final的值也应该返回去
*/
arma::uvec geometricVerification(const arma::mat &frames1, const arma::mat &frames2,
const arma::mat &matches, const superluOpts &opts){
// 测试载入是否准确
/*std::cout<< "element测试: " << " x: " << frames1(0,1) << " y: " << frames1(1,1) << std::endl;
std::cout << " 行数： " << frames1.n_rows << " 列数：" << frames1.n_cols << std::endl;
std::cout << "==========================================================" << std::endl;*/
int numMatches = matches.n_cols;
// 测试匹配数目是否准确
/*std::cout << "没有RANSAC前匹配数目： " << numMatches << std::endl;
std::cout << "==========================================================" << std::endl;*/
arma::field<arma::uvec> inliers(1, numMatches);
arma::field<arma::mat> H(1, numMatches);
arma::uvec v = arma::linspace<arma::uvec>(0,1,2);
arma::mat onesVector = arma::ones(1, matches.n_cols);
arma::uvec matchedIndex_Query = arma::conv_to<arma::uvec>::from(matches.row(0)-1);
arma::uvec matchedIndex_Object = arma::conv_to<arma::uvec>::from(matches.row(1)-1);
arma::mat x1 = frames1(v, matchedIndex_Query) ;
arma::mat x2 = frames2(v, matchedIndex_Object);
/*std::cout << " x1查询图像匹配行数： " << x1.n_rows << " 查询图像匹配列数：" << x1.n_cols << std::endl;
std::cout << " x2目标图像匹配行数： " << x2.n_rows << " 目标图像匹配列数：" << x2.n_cols << std::endl;
std::cout<< "x1 element测试: " << " x: " << x1(0,1) << " y: " << x1(1,1) << std::endl;
std::cout<< "x2 element测试: " << " x: " << x2(0,1) << " y: " << x2(1,1) << std::endl;
std::cout << "==========================================================" << std::endl;*/
arma::mat x1hom = arma::join_cols(x1, arma::ones<arma::mat>(1, numMatches)); //在下面添加一行，注意和join_rows的区别
arma::mat x2hom = arma::join_cols(x2, arma::ones<arma::mat>(1, numMatches));
/*std::cout << " x1hom查询图像匹配行数： " << x1hom.n_rows << " 查询图像匹配列数：" << x1hom.n_cols << std::endl;
std::cout<< "x1hom element测试: " << " x: " << x1hom(0,1) << " y: " << x1hom(1,1) << " z: " << x1hom(2,1) << std::endl;
std::cout << "==========================================================" << std::endl;*/
arma::mat x1p, H21; //作用域
double tol;
for(int m = 0; m < numMatches; ++m){
//cout << "m: " << m << endl;
for(unsigned int t = 0; t < opts.numRefinementIterations; ++t){
//cout << "t: " << t << endl;
if (t == 0){
arma::mat tmp1 = frames1.col(matches(0, m)-1);
arma::mat A1 = toAffinity(tmp1);
//A1.print("A1 =");
arma::mat tmp2 = frames2.col(matches(1, m)-1);
arma::mat A2 = toAffinity(tmp2);
//A2.print("A2 =");
H21 = A2 * inv(A1);
//H21.print("H21 =");
x1p = H21.rows(0, 1) * x1hom ;
//x1p.print("x1p =");
tol = opts.tolerance1;
}else if(t !=0 && t <= 3){
arma::mat A1 = x1hom.cols(inliers(0, m));
arma::mat A2 = x2.cols(inliers(0, m));
//A1.print("A1 =");
//A2.print("A2 =");
H21 = A2*pinv(A1);
//H21.print("H21 =");
x1p = H21.rows(0, 1) * x1hom ;
//x1p.print("x1p =");
arma::mat v;
v << 0 << 0 << 1 << arma::endr;
H21 = join_vert(H21, v);
//H21.print("H21 =");
//x1p.print("x1p =");
tol = opts.tolerance2;
}else{
arma::mat x1in = x1hom.cols(inliers(0, m));
arma::mat x2in = x2hom.cols(inliers(0, m));
arma::mat S1 = centering(x1in);
arma::mat S2 = centering(x2in);
arma::mat x1c = S1 * x1in;
//x1c.print("x1c =");
arma::mat x2c = S2 * x2in;
//x2c.print("x2c =");
arma::mat A1 = arma::randu<arma::mat>(x1c.n_rows ,x1c.n_cols);
A1.zeros();
arma::mat A2 = arma::randu<arma::mat>(x1c.n_rows ,x1c.n_cols);
A2.zeros();
arma::mat A3 = arma::randu<arma::mat>(x1c.n_rows ,x1c.n_cols);
A3.zeros();
for(unsigned int i = 0; i < x1c.n_cols; ++i){
A2.col(i) = x1c.col(i)*(-x2c.row(0).col(i));
A3.col(i) = x1c.col(i)*(-x2c.row(1).col(i));
}
arma::mat T1 = join_cols(join_horiz(x1c, A1), join_horiz(A1, x1c));
arma::mat T2 = join_cols(T1, join_horiz(A2, A3));
//T2.print("T2 =");
arma::mat U;
arma::vec s;
arma::mat V;
svd_econ(U, s, V, T2);
//U.print("U =");
//V.print("V =");
arma::vec tmm = U.col(U.n_cols-1);
H21 = reshape(tmm, 3, 3).t();
H21 = inv(S2) * H21 * S1;
H21 = H21 / H21(H21.n_rows-1, H21.n_cols-1) ;
//H21.print("H21 =");
arma::mat x1phom = H21 * x1hom ;
arma::mat cc1 = x1phom.row(0) / x1phom.row(2);
arma::mat cc2 = x1phom.row(1) / x1phom.row(2);
arma::mat x1p = join_cols(cc1, cc2);
//x1p.print("x1p =");
tol = opts.tolerance3;
}
arma::mat tmp = arma::square(x2 - x1p); //精度跟matlab相比更高？
//tmp.print("tmp =");
arma::mat dist2 = tmp.row(0) + tmp.row(1);
//dist2.print("dist2 =");
inliers(0, m) = arma::find(dist2 < pow(tol, 2));
H(0, m) = H21;
//H(0, m).print("H(0, m) =");
//inliers(0, m).print("inliers(0, m) =");
//cout << inliers(0, m).size() << endl;
//cout << "==========================================================" << endl;
if (inliers(0, m).size() < opts.minInliers) break;
if (inliers(0, m).size() > 0.7 * numMatches) break;
}
}
arma::uvec scores(numMatches);
for(int i = 0; i < numMatches; ++i){
scores.at(i) = inliers(0, i).n_rows;
}
//scores.print("scores = ");
arma::uword index;
scores.max(index);
//cout << index << endl;
arma::mat H_final = inv(H(0, index));
//H_final.print("H_final = ");
arma::uvec inliers_final = inliers(0, index);
//inliers_final.print("inliers_final = ");
return inliers_final;
}
