C++ SpMat类代码示例

inline
void
op_sp_plus::apply_inside_schur(SpMat<eT>& out, const T2& x, const SpToDOp<T3, op_sp_plus>& y)
  {
  arma_extra_debug_sigprint();

  const SpProxy<T2> proxy2(x);
  const SpProxy<T3> proxy3(y.m);

  arma_debug_assert_same_size(proxy2.get_n_rows(), proxy2.get_n_cols(), proxy3.get_n_rows(), proxy3.get_n_cols(), "element-wise multiplication");

  out.zeros(proxy2.get_n_rows(), proxy2.get_n_cols());
  
  typename SpProxy<T2>::const_iterator_type it     = proxy2.begin();
  typename SpProxy<T2>::const_iterator_type it_end = proxy2.end();
  
  const eT k = y.aux;
  
  for(; it != it_end; ++it)
    {
    const uword it_row = it.row();
    const uword it_col = it.col();
    
    out.at(it_row, it_col) = (*it) * (proxy3.at(it_row, it_col) + k);
    }
  }

inline
void
op_sp_plus::apply(SpMat<typename T1::elem_type>& out, const SpToDOp<T1,op_sp_plus>& in)
  {
  arma_extra_debug_sigprint();

  typedef typename T1::elem_type eT;

  // Note that T1 will be a sparse type, so we use SpProxy.
  const SpProxy<T1> proxy(in.m);
  
  const uword n_rows = proxy.get_n_rows();
  const uword n_cols = proxy.get_n_cols();
  
  out.set_size(n_rows, n_cols);
  
  const eT k = in.aux;
  
  // We have to loop over all the elements.
  for(uword c = 0; c < n_cols; ++c)
  for(uword r = 0; r < n_rows; ++r)
    {
    out.at(r, c) = proxy.at(r, c) + k;
    }
  }

void ADMMCut::CalculateX()
{
	// Construct Hessian Matrix for D-Qp problem
	SpMat Q;
	CalculateQ(D_, Q);

	SpMat H2 = Q.transpose() * Q;
	SpMat H = 2 * H1_ + penalty_ * H2;

	// Construct Linear coefficient for x-Qp problem
	a_ = Q.transpose() * lambda_;

	// Inequality constraints A*x <= b
	SpMat A(6 * Fd_, 6 * Fd_);
	A.setZero();
	VX b(6 * Fd_);
	b.setZero();

	// Variable constraints x >= lb, x <= ub
	VX lb(Nd_), ub(Nd_);
	lb.setZero();
	ub.setOnes();

	qp_->solve(H, a_, A, b, W_, d_, lb, ub, x_, NULL, NULL, debug_);
}

inline void write_beta_matrix(SpMat &betas, int col, double beta0, SpVec &coef)
{
    betas.insert(0, col) = beta0;

    for(SpVec::InnerIterator iter(coef); iter; ++iter)
    {
        betas.insert(iter.index() + 1, col) = iter.value();
    }
}

bool StiffnessSolver::SolveSystem(
		SpMat &K, VX &D, VX &F, VX &R, 
		int DoF, VXi &q, VXi &r, 
		int verbose, int &info, double &rms_resid)
{
	VX	diag;		/* diagonal vector of the L D L' decomp. */

	diag.resize(DoF);

	//MX K_comp = K;
	int row = K.rows(), col = K.cols();
	MX K_comp(row, col);
	K_comp.setZero();

	for (int k = 0; k < K.outerSize(); ++k)
	{
		for (SpMat::InnerIterator it(K, k); it; ++it)
		{
			int		r = it.row();
			int		c = it.col();
			double	v = it.value();
			K_comp(r, c) = v;
		}
	}

	/*  L D L' decomposition of K[q,q] into lower triangle of K[q,q] and diag[q] */
	/*  vectors F and D are unchanged */
	/*  not solving at this moment*/
	LDLDecompPM(K_comp, DoF, diag, F, D, R, q, r, 1, 0, info);
	if (info < 0)
	{
		fprintf(stderr, "Stiffness Matrix is not positive definite: %d negative elements\n", info);
		fprintf(stderr, "found on decomp diagonal of K.\n");
		fprintf(stderr, "The stucture may have mechanism and thus not stable in general\n");
		fprintf(stderr, "Please Make sure that all six\n");
		fprintf(stderr, "rigid body translations are restrained!\n");
		
		return false;
	}
	else
	{
		/* LDL'  back-substitution for D[q] and R[r] */
		LDLDecompPM(K_comp, DoF, diag, F, D, R, q, r, 0, 1, info);
		if (verbose) { fprintf(stdout, "    LDL' RMS residual:"); }
		rms_resid = info = 1;

		do {
			/* improve solution for D[q] and R[r] */
			LDLImprovePM(K_comp, DoF, diag, F, D, R, q, r, rms_resid, info);
			if (verbose) { fprintf(stdout, "%9.2e", rms_resid); }
		} while (info);

		if (verbose) fprintf(stdout, "LDL^t Solving completed\n");
	}

	return true;
}

inline
void
spop_repmat::apply(SpMat<typename T1::elem_type>& out, const SpOp<T1, spop_repmat>& in)
  {
  arma_extra_debug_sigprint();
  
  typedef typename T1::elem_type eT;
  
  const unwrap_spmat<T1> U(in.m);
  const SpMat<eT>& X =   U.M;
  
  const uword X_n_rows = X.n_rows;
  const uword X_n_cols = X.n_cols;
  
  const uword copies_per_row = in.aux_uword_a;
  const uword copies_per_col = in.aux_uword_b;
  
  // out.set_size(X_n_rows * copies_per_row, X_n_cols * copies_per_col);
  // 
  // const uword out_n_rows = out.n_rows;
  // const uword out_n_cols = out.n_cols;
  // 
  // if( (out_n_rows > 0) && (out_n_cols > 0) )
  //   {
  //   for(uword col = 0; col < out_n_cols; col += X_n_cols)
  //   for(uword row = 0; row < out_n_rows; row += X_n_rows)
  //     {
  //     out.submat(row, col, row+X_n_rows-1, col+X_n_cols-1) = X;
  //     }
  //   }
  
  SpMat<eT> tmp(X_n_rows * copies_per_row, X_n_cols);
  
  if(tmp.n_elem > 0)
    {
    for(uword row = 0; row < tmp.n_rows; row += X_n_rows)
      {
      tmp.submat(row, 0, row+X_n_rows-1, X_n_cols-1) = X;
      }
    }
  
  // tmp contains copies of the input matrix, so no need to check for aliasing
  
  out.set_size(X_n_rows * copies_per_row, X_n_cols * copies_per_col);
  
  const uword out_n_rows = out.n_rows;
  const uword out_n_cols = out.n_cols;
  
  if( (out_n_rows > 0) && (out_n_cols > 0) )
    {
    for(uword col = 0; col < out_n_cols; col += X_n_cols)
      {
      out.submat(0, col, out_n_rows-1, col+X_n_cols-1) = tmp;
      }
    }
  }

inline
void
SpSubview<eT>::eye()
  {
  arma_extra_debug_sigprint();
  
  SpMat<eT> tmp;
  
  tmp.eye( (*this).n_rows, (*this).n_cols );
  
  (*this).operator=(tmp);
  }

inline void write_beta_matrix(SpMat &betas, int col, double beta0, SpVec &coef, bool startatzero)
{
    
    int add = 0;
    if (!startatzero)
    {
        add = 1;
        betas.insert(0, col) = beta0;
    }
    for(SpVec::InnerIterator iter(coef); iter; ++iter)
    {
        betas.insert(iter.index() + add, col) = iter.value();
    }
}

inline void eliminationOperators(SpMat& A, DynamicVector<size_t>& Cset, size_t fnode,
        DynamicVector<double>& q, SpMat& P, size_t& P_col, size_t P_row) {
    /* Inizializziamo la riga P_row con A.nonZeros(fnode) - 1 non nulli*/
    double scalingFactor = 1.0;
    /* Riserviamo spazio in ciascuna colonna di P per un adeguato numero
     * di elementi. Il -1 c'è perché (il reciproco del)l'elemento in
     * diagonale lo mettiamo in q
     */
    P.reserve(P_row, A.nonZeros(fnode) - 1);
    /* Per ciascuna f-riga prendo gli elementi in ogni c-colonna */
    DynamicVector<size_t>::Iterator ccol = Cset.begin();
    for (SpMat::Iterator frow = A.begin(fnode); frow != A.end(fnode); ++frow) {
        if (frow->index() == fnode) { //Elemento della diagonale
            q[P_row] = (1.0 / frow->value()); //Q ha elementi pari al numero di righe di R
            scalingFactor = -(q[P_row]);
        } else if (ccol != Cset.end()) {
            break; //Non ha senso andare avanti se abbiamo finito i ccol
        } else if (frow->index() == (*ccol)) {
            /* Elemento fuori della diagonale ed è anche un c-colonna */
            P.append(P_row, P_col, frow->value());
            P_col++;
            ccol++;
        }
    }

    P.finalize(P_row); //Finalizziamo la riga P_row

    /* Non dimentichiamo di scalare gli elementi della riga corrente
     * per  -scalingFactor */
    for (SpMat::Iterator it = P.begin(P_row); it != P.end(P_row); it++)
        it->value() *= -scalingFactor;
}

Mat solveQurdOpt(SpMat L, SpMat C, Mat alpha_star){
	//
	cout << "solving quadratic optimization proble .............." << endl;

	double lambda = 0.000001;

	SpMat D(L.rows(), L.cols());
	D.setIdentity();
	//cout << D << endl;

	alpha_star = matlab_colVector(alpha_star);
	MatrixXd as_dense;
	cv2eigen(alpha_star, as_dense);
	SpMat b = as_dense.sparseView();


	SpMat A, alpha;
	A = L + C + lambda * D;
	b = C * b;
	//cout << b << endl; 

	Eigen::SimplicialLLT<SpMat> solver;
	//Eigen::SimplicialLDLT<SpMat> solver;
	//Eigen::SparseQR<Eigen::SparseMatrix<double>> solver;
	//Eigen::BiCGSTAB<SpMat> solver;
	solver.compute(A);
	if (solver.info() != Eigen::Success) {
		cout << "decomposition failed" << endl;
	}
	cout << "decomposition success" << endl;

	cout << "begin to solve !" << endl;
	alpha = solver.solve(b);
	cout << "solve success" << endl;

	Mat cvAlpha;
	eigen2cv(Eigen::MatrixXd(alpha), cvAlpha);

	cvAlpha = cvAlpha.reshape(0, sz.width);
	cvAlpha = cvAlpha.t();

	showSavePicLBDM("alpha", cvAlpha, showImgLBDM, saveImgLBDM);

	cvAlpha = cvAlpha*0.5 + 0.5;

	cvAlpha = max(min(cvAlpha, 1.0), 0.0);

	return cvAlpha;
}

arma_hot
inline
void
spglue_plus::apply(SpMat<typename T1::elem_type>& out, const SpGlue<T1,T2,spglue_plus>& X)
  {
  arma_extra_debug_sigprint();
  
  typedef typename T1::elem_type eT;
  
  const SpProxy<T1> pa(X.A);
  const SpProxy<T2> pb(X.B);
  
  const bool is_alias = pa.is_alias(out) || pb.is_alias(out);
  
  if(is_alias == false)
    {
    spglue_plus::apply_noalias(out, pa, pb);
    }
  else
    {
    SpMat<eT> tmp;
    spglue_plus::apply_noalias(tmp, pa, pb);
    
    out.steal_mem(tmp);
    }
  }

inline
void
spop_var::apply(SpMat<typename T1::pod_type>& out, const mtSpOp<typename T1::pod_type, T1, spop_var>& in)
  {
  arma_extra_debug_sigprint();
  
  //typedef typename T1::elem_type  in_eT;
  typedef typename T1::pod_type  out_eT;
  
  const uword norm_type = in.aux_uword_a;
  const uword dim       = in.aux_uword_b;
  
  arma_debug_check((norm_type > 1), "var(): incorrect usage. norm_type must be 0 or 1");
  arma_debug_check((dim > 1),       "var(): incorrect usage. dim must be 0 or 1");
  
  SpProxy<T1> p(in.m);
  
  if(p.is_alias(out) == false)
    {
    spop_var::apply_noalias(out, p, norm_type, dim);
    }
  else
    {
    SpMat<out_eT> tmp;
    
    spop_var::apply_noalias(tmp, p, norm_type, dim);
    
    out.steal_mem(tmp);
    }
  }

SpMat make_C(SpMat chol_K_inv,VectorXd h2s, SpMat ZtZ){
  SpMat Ki = chol_K_inv.transpose() * chol_K_inv;
  SpMat C = ZtZ;
  C /= (1.0 - h2s.sum());
  C += Ki;
  return C;
}

inline
void
spop_mean::apply(SpMat<typename T1::elem_type>& out, const SpOp<T1, spop_mean>& in)
  {
  arma_extra_debug_sigprint();
  
  typedef typename T1::elem_type eT;
  
  const uword dim = in.aux_uword_a;
  arma_debug_check( (dim > 1), "mean(): parameter 'dim' must be 0 or 1" );
  
  const SpProxy<T1> p(in.m);
  
  if(p.is_alias(out) == false)
    {
    spop_mean::apply_noalias_fast(out, p, dim);
    }
  else
    {
    SpMat<eT> tmp;
    
    spop_mean::apply_noalias_fast(tmp, p, dim);
    
    out.steal_mem(tmp);
    }
  }

inline
void
spop_sqrt::apply(SpMat<typename T1::elem_type>& out, const SpOp<T1,spop_sqrt>& in)
  {
  arma_extra_debug_sigprint();
  
  out.init_xform(in.m, priv::functor_sqrt());
  }