SXMatrix spy(const SXMatrix& A){ SXMatrix s(A.size1(),A.size2()); for(int i=0; i<A.size1(); ++i) for(int j=0; j<A.size2(); ++j) if(!A(i,j).toScalar()->isZero()) s(i,j) = 1; return s; }
void MaterialFitting::solveByNNLS(){ const SXMatrix M1 = assembleObjMatrix(); vector<SX> smooth_funs; computeSmoothObjFunctions(smooth_funs); SXMatrix obj_fun_m(M1.size1()*M1.size2()+smooth_funs.size(),1); for (int i = 0; i < M1.size1(); ++i){ for (int j = 0; j < M1.size2(); ++j) obj_fun_m.elem(i*M1.size2()+j,0) = M1.elem(i,j); } for (int i = 0; i < smooth_funs.size(); ++i){ obj_fun_m.elem(M1.size2()*M1.size2()+i,0) = smooth_funs[i]; } VSX variable_x; initAllVariables(variable_x); CasADi::SXFunction g_fun = CasADi::SXFunction(variable_x,obj_fun_m); g_fun.init(); VectorXd x0(variable_x.size()), b; x0.setZero(); CASADI::evaluate(g_fun, x0, b); b = -b; MatrixXd J = CASADI::convert<double>(g_fun.jac()); assert_eq(J.rows(), b.size()); assert_eq(J.cols(), x0.size()); VectorXd x(b.size()), w(J.cols()), zz(J.rows()); VectorXi index(J.cols()*2); getInitValue(x); int exit_code = 0; double residual = 1e-18; nnls(&J(0,0), J.rows(), J.rows(), J.cols(), &b[0], &x[0], &residual, &w[0], &zz[0], &index[0], &exit_code); INFO_LOG("residual: " << residual); print_NNLS_exit_code(exit_code); rlst.resize(x.size()); for (int i = 0; i < x.size(); ++i){ rlst[i] = x[i]; } const MatrixXd H = J.transpose()*J; SelfAdjointEigenSolver<MatrixXd> es; es.compute(H); cout << "The eigenvalues of H are: " << es.eigenvalues().transpose() << endl; }
void fill(SXMatrix& mat, const SX& val){ if(val->isZero()) mat.makeEmpty(mat.size1(),mat.size2()); else mat.makeDense(mat.size1(),mat.size2(),val); }
int getIntValue(const SXMatrix& ex, int i, int j) { casadi_assert(i<ex.size1() && j<ex.size2()); return ex(i,j).toScalar().getIntValue(); }
int sxMatrixSize2(const SXMatrix & mat) { return mat.size2(); }