/******************************************************************
* 函数功能：几何校正需要调用的函数
*
*
*/
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;
}
