/****************************************************************** * 函数功能:几何校正需要调用的函数 * * */ arma::mat centering(arma::mat &x){ arma::mat tmp = -mean(x.rows(0, 1), 1); arma::mat tmp2(2,2); tmp2.eye(); arma::mat tmp1 = join_horiz(tmp2, tmp); arma::mat v; v << 0 << 0 << 1 << arma::endr; arma::mat T = join_vert(tmp1, v); //T.print("T ="); arma::mat xm = T * x; //xm.print("xm ="); //at least one pixel apart to avoid numerical problems //xm.print("xm ="); double std11 = arma::stddev(xm.row(0)); //cout << "std11:" << std11 << endl; double std22 = stddev(xm.row(1)); //cout << "std22:" << std22 << endl; double std1 = std::max(std11, 1.0); double std2 = std::max(std22, 1.0); arma::mat S; S << 1/std1 << 0 << 0 << arma::endr << 0 << 1/std2 << 0 << arma::endr << 0 << 0 << 1 << arma::endr; arma::mat C = S * T ; //C.print("C ="); return C; }
/// /// \brief UnivariateData::Calibrate /// \param values Intensity (or band ratio, or areas) of calibration curve) /// \param conentrations /// void UnivariateData::Calibrate(const vec &values, const vec &concentrations) { mat X = Vespucci::Math::LinLeastSq::Vandermonde(concentrations, 1); vec calibration_y; vec coefs = Vespucci::Math::LinLeastSq::OrdinaryLeastSquares(X, values, calibration_y, calibration_stats_); calibration_stats_["Calibrated"] = 1; vec residuals = values - calibration_y; calibration_curve_ = join_horiz(concentrations, join_horiz(calibration_y, residuals)); double b = coefs(0); double m = coefs(1); results_.transform([m, b](double val){return (val - b)/m;}); }
/******************************************************************* * 函数功能:几何校正需要调用的函数 * * */ arma::mat toAffinity(arma::mat &f){ arma::mat A; arma::mat v; v << 0 << 0 << 1 << arma::endr; int flag = f.n_rows; switch(flag){ case 6:{ // oriented ellipses arma::mat T = f.rows(0, 1); arma::mat tmp = join_horiz(f.rows(2, 3), f.rows(4, 5)); arma::mat tmp1 = join_horiz(tmp, T); A = join_vert(tmp1, v); break;} case 4:{ // oriented discs arma::mat T = f.rows(0, 1); double s = f.at(2,0); double th = f.at(3,0); arma::mat S = arma::randu<arma::mat>(2,2); /*S.at(0, 0) = s*cos(th); S.at(0, 1) = -s*sin(th); S.at(1, 0) = s*sin(th); S.at(1, 1) = s*cos(th);*/ S << s*cos(th) << -s*sin(th) << arma::endr << s*sin(th) << s*cos(th) << arma::endr; arma::mat tmp1 = join_horiz(S, T); A = join_vert(tmp1, v); //A.print("A ="); break;} /*case 3:{ // discs mat T = f.rows(0, 1); mat s = f.row(2); int th = 0 ; A = [s*[cos(th) -sin(th) ; sin(th) cos(th)], T ; 0 0 1] ; } case 5:{ // ellipses mat T = f.rows(0, 1); A = [mapFromS(f(3:5)), T ; 0 0 1] ; }*/ default: std::cout << "出错啦!" << std::endl; break; } return A; }
vector<mat> make_raw_freebie_flow_bases(const mat & value_basis, const vector<sp_mat> blocks, const mat & Q){ // Same as above, but don't orthonormalize vector<mat> flow_bases; uint A = blocks.size(); assert((A+1) == Q.n_cols); for(uint a = 0; a < A; a++){ mat raw_basis = join_horiz(mat(blocks.at(a).t() * value_basis), Q.col(a+1)); flow_bases.push_back(raw_basis); } return flow_bases; }
vector<sp_mat> make_freebie_flow_bases(const sp_mat & value_basis, const vector<sp_mat> blocks, const mat & Q){ vector<sp_mat> flow_bases; uint A = blocks.size(); assert((A+1) == Q.n_cols); for(uint a = 0; a < A; a++){ mat raw_basis = join_horiz(mat(blocks.at(a).t() * value_basis), Q.col(a+1)); flow_bases.push_back(sp_mat(orth(raw_basis))); // Orthonorm (TODO: do directly in sparse?) } return flow_bases; }
/// /// \brief AnalysisResults::Concatenate /// \param other /// Merge the matrices of other into the same-named matrices of this one /// Will perform a check of mergability for every matrix before performing merge /// Non mergable matrices are removed from this result. bool AnalysisResults::Concatenate(QSharedPointer<AnalysisResults> other) { QSet<QString> intersection = KeyList().toSet().intersect(other->KeyList().toSet()); //Check mergability (this should not be a problem if both come from same dataset) //User may be allowed to merge from two different datasets, if for some reason they wanted to do that QSet<QString> mergeable_matrices = intersection; for (auto key: intersection) if (GetMatrix(key).n_rows != other->GetMatrix(key).n_rows) { mergeable_matrices.remove(key); RemoveMatrix(key); } if (mergeable_matrices.isEmpty()) return false; for (auto key: mergeable_matrices) { mat matrix = GetMatrix(key); mat other_matrix = other->GetMatrix(key); matrix = join_horiz(matrix, other_matrix); AddMatrix(key, matrix); } return true; }
Dy* _5R_::Doit(vec q0_, vec q1_){ this->q0_ = q0_; this->q1_ = q1_; P->Doit(q0_(span(0,P->dof-1)), q1_(span(0,P->dof-1))); uint soma = P->dof; for(uint i=0; i<nR_; i++){ soma += R_[i]->dof; R_[i]->Doit(q0_(span(soma-R_[i]->dof,soma-1)), q1_(span(soma-R_[i]->dof,soma-1)) ); } //M_, v_ e g_ M_ = join_diag(M__, nR_+1); v_ = join_vert(v__, nR_+1); g_ = join_vert(g__, nR_+1); // _q_, Ah_, Ao_, A_, b_, C_ e Z_ _q_ = D_*P->q0_ - E_*join_vert(o__, nR_) - F_*join_vert(q0__, nR_) - f_; switch(caso){ case 1: Ao_ = -E_*join_diag(Jv_n__, nR_) - F_; b_ = E_*join_vert(a_co_n__, nR_); break; case 2: Ao_ = join_vert(-E_*join_diag(Jv_n__, nR_) - F_, -Q_*join_diag(Jw_n__, nR_) - S_); b_ = join_vert(E_*join_vert(a_co_n__, nR_), F_*join_vert(dw_co_n__, nR_)); break; } A_ = join_horiz(Ah_, Ao_); C_ = join_vert( eye(dof,dof), -solve(Ao_,Ah_) ); mat Aux_ = C_.row(u_nzi_(0)); for(uint i=1; i<u_nzi_.n_rows; i++) Aux_ = join_vert(Aux_, C_.row(u_nzi_(i))); Z_ = Aux_; //Z_ = join_vert(C_.row(2), C_.row(4)); // Mh_, vh_ e gh_ dy->Mh_ = solve(Z_.t(), C_.t()*M_*C_); dy->vh_ = solve(Z_.t(), C_.t()*( M_*join_vert(zeros(dof), solve(Ao_, b_) ) + v_ )); dy->gh_ = solve(Z_.t(), C_.t()*g_); return dy; }
/****************************************************************** * 函数功能:几何校正 * * 待写: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; }
int main(){ Mecanismo P = Mecanismo(2); cube I1__; I1__.zeros(3,3,2); I1__.slice(0) << 0 << 0 << 0 << endr << 0 << 107.307e-6 + 146.869e-6 << 0 << endr << 0 << 0 << 107.307e-6 + 146.869e-6 << endr; I1__.slice(1) << 0 << 0 << 0 << endr << 0 << 438.0e-6 << 0 << endr << 0 << 0 << 438.0e-6 << endr; cube I2__; I2__.zeros(3,3,2); I2__.slice(0) << 0 << 0 << 0 << endr << 0 << 107.307e-6 + 188.738e-6 << 0 << endr << 0 << 0 << 107.307e-6 + 188.738e-6 << endr; I2__.slice(1) << 0 << 0 << 0 << endr << 0 << 301.679e-6 << 0 << endr << 0 << 0 << 301.679e-6 << endr; Serial RR1 = Serial(2, {0.12, 0.16}, {0.06, 0.078},{0.062, 0.124}, I1__ , {0, 0, 9.8}, &fDH_RR); Serial RR2 = Serial(2, {0.12, 0.16}, {0.06, 0.058},{0.062, 0.097}, I2__ , {0, 0, 9.8}, &fDH_RR); Serial **RR_ = new Serial* [2]; RR_[0] = &RR1; RR_[1] = &RR2; //Matrizes que descrevem a arquitetura do mecanismo double l0 = 0.05; mat D_ = join_vert((mat)eye(2,2),2); mat E_ = join_diag( Roty(0)(span(0,1),span(0,2)), Roty(PI)(span(0,1),span(0,2)) ); mat F_ = zeros(4,4); vec f_ = {l0,0,-l0,0}; Parallel Robot = Parallel(2, &P, RR_, 2, {2,4}, D_, E_, F_, f_); Reference RefObj = Reference(0.12, {0.08, 0.16}, {-0.08, 0.4}); //Plotar área de trabalho uint nx = 96.0; uint ny = 56.0; double lx = 0.24; double ly = 0.28; double xi = -lx; double xf = lx; double yi = 0.0; double yf = ly; double dx = (xf-xi)/(nx-1); double dy = (yf-yi)/(ny-1); double dl = 0.5*(dx+dy); Mat<int> M; M.zeros(nx,ny); field<mat> fZ_(nx,ny); field<mat> fMh_(nx,ny); field<vec> fgh_(nx,ny); field<vec> fa1_(nx,ny); field<vec> fa2_(nx,ny); field<vec> fa12_(nx,ny); for(uint i=0; i<nx; i++){ for(uint j=0; j<ny; j++){ fZ_(i,j).zeros(2,2); fMh_(i,j).zeros(2,2); fgh_(i,j).zeros(2); fa1_(i,j).zeros(2); fa2_(i,j).zeros(2); fa12_(i,j).zeros(2); } } vec v1_ = {1,0}; vec v2_ = {0,1}; vec v12_ = {1,1}; double r = 0.07; double x0 = 0.0; double y0 = 0.17; uint rows = nx; uint cols = ny; vec q0_ = {0.823167, 1.81774, 0.823167, 1.81774}; GNR2 gnr2 = GNR2("RK6", &Robot, 1e-6, 30); //gnr2.Doit(q0_, {0.05,0.08}); //cout << gnr2.convergiu << endl; //cout << gnr2.x_ << endl; //cout << gnr2.res_ << endl; //cout << gnr2.n << endl; double x; double y; mat A2_; for(uint i=0; i<rows; i++){ for(uint j=0; j<cols; j++){ x = xi + i*dx; y = yi + j*dy; gnr2.Doit(q0_, {x, y}); if(gnr2.convergiu){ q0_ = gnr2.x_; A2_ = join_horiz(Robot.Ah_, join_horiz(Robot.Ao_.col(1), Robot.Ao_.col(3)) ); if(abs(det(Robot.Ao_)) < 1.6*1e-6 || abs(det(A2_)) < 1e-11 ) M(i,j) = 2; else{ M(i,j) = 1; Robot.Doit(Robot.q0_, join_vert(v1_, -solve(Robot.Ao_, Robot.Ah_*v1_)) ); fZ_(i,j) = Robot.Z_; fMh_(i,j) = Robot.dy->Mh_; fgh_(i,j) = Robot.dy->gh_; fa1_(i,j) = Robot.dy->vh_; Robot.Doit(Robot.q0_, Robot.C_*v2_); fa2_(i,j) = Robot.dy->vh_; Robot.Doit(Robot.q0_, Robot.C_*v12_); fa12_(i,j) = Robot.dy->vh_ - fa1_(i,j) - fa2_(i,j); } } gnr2.convergiu = false; if( ((x - x0)*(x - x0) + (y - y0)*(y - y0) <= (r+dl)*(r+dl) ) && ((x - x0)*(x - x0) + (y - y0)*(y - y0) >= (r-dl)*(r-dl) ) && (M(i,j) != 2) ) M(i,j) = 3; } } for(uint i=0; i<rows; i++){ for(uint j=0; j<cols; j++){ cout << M(i,j) << ";" ; if(j==cols-1) cout << endl; } } fZ_.save("fZ_field"); fMh_.save("fMh_field"); fgh_.save("fgh_field"); fa1_.save("fa1_field"); fa2_.save("fa2_field"); fa12_.save("fa12_field"); return 0; }
int dtq::compProp(void) { // need myh to proceed if (! haveMyh) return 1; // fundamental size ylen = 2*bigm+1; // start computing the propagator! prop = arma::sp_mat(ylen,ylen); // create yvec yvec = arma::zeros(ylen); for (int i=-bigm; i<=bigm; i++) yvec(bigm+i) = i*myk; // apply f and g to yvec fy = arma::zeros(ylen); gy = arma::zeros(ylen); for (int i=-bigm; i<=bigm; i++) { fy(bigm+i) = (*f)(yvec(bigm+i),curtheta); gy(bigm+i) = (*g)(yvec(bigm+i),curtheta); } // normalization "constant" arma::vec c0mod = 1.0/(sqrt(2.0*(arma::datum::pi)*myh)*gy); // variance arma::vec gy2 = gy % gy; arma::vec myvar = gy2*myh; // compute and set main diagonal // prop.diag() = maindiag; arma::vec propvals = arma::exp(-(myh/2.0)*(fy%fy)/gy2) % c0mod; arma::umat proploc(2, ylen); proploc.row(0) = arma::regspace<arma::urowvec>(0, (ylen-1)); proploc.row(1) = arma::regspace<arma::urowvec>(0, (ylen-1)); // superdiagonals bool done = false; int curdiag = 1; double mytol = 2.0e-16; double refsum = arma::sum(arma::abs(propvals))*myk; while (! done) { arma::vec mymean = curdiag*myk + fy*myh; arma::vec thisdiag = arma::exp(-mymean%mymean/(2.0*myvar))%c0mod; thisdiag = thisdiag.tail(ylen - curdiag); double thissum = arma::sum(arma::abs(thisdiag)); if ((curdiag == 1) || (thissum > mytol*refsum)) { // prop.diag(curdiag) = thisdiag; arma::umat newloc(2, ylen-curdiag); newloc.row(0) = arma::regspace<arma::urowvec>(0, (ylen-curdiag-1)); newloc.row(1) = newloc.row(0) + curdiag; proploc = join_horiz(proploc, newloc); propvals = join_vert(propvals, thisdiag); curdiag++; if (curdiag == ylen) done = true; } else done = true; } int maxdiag = curdiag; for (curdiag=1; curdiag<=maxdiag; curdiag++) { arma::vec mymean = -curdiag*myk + fy*myh; arma::vec thisdiag = arma::exp(-mymean%mymean/(2.0*myvar))%c0mod; thisdiag = thisdiag.head(ylen - curdiag); // prop.diag(-curdiag) = thisdiag; arma::umat newloc(2, ylen-curdiag); newloc.row(1) = arma::regspace<arma::urowvec>(0, (ylen-curdiag-1)); newloc.row(0) = newloc.row(1) + curdiag; proploc = join_horiz(proploc, newloc); propvals = join_vert(propvals, thisdiag); } prop = arma::sp_mat(proploc, propvals, ylen, ylen); // check normalization, should get all 1's // std::cout << myk*sum(prop,0) << '\n'; haveProp = true; return 0; }