コード例 #1
0
ファイル: Polygon.cpp プロジェクト: ethz-asl/grid_map
bool Polygon::convertToInequalityConstraints(Eigen::MatrixXd& A, Eigen::VectorXd& b) const
{
  Eigen::MatrixXd V(nVertices(), 2);
  for (unsigned int i = 0; i < nVertices(); ++i)
    V.row(i) = vertices_[i];

  // Create k, a list of indices from V forming the convex hull.
  // TODO: Assuming counter-clockwise ordered convex polygon.
  // MATLAB: k = convhulln(V);
  Eigen::MatrixXi k;
  k.resizeLike(V);
  for (unsigned int i = 0; i < V.rows(); ++i)
    k.row(i) << i, (i+1) % V.rows();
  Eigen::RowVectorXd c = V.colwise().mean();
  V.rowwise() -= c;
  A = Eigen::MatrixXd::Constant(k.rows(), V.cols(), NAN);

  unsigned int rc = 0;
  for (unsigned int ix = 0; ix < k.rows(); ++ix) {
    Eigen::MatrixXd F(2, V.cols());
    F.row(0) << V.row(k(ix, 0));
    F.row(1) << V.row(k(ix, 1));
    Eigen::FullPivLU<Eigen::MatrixXd> luDecomp(F);
    if (luDecomp.rank() == F.rows()) {
      A.row(rc) = F.colPivHouseholderQr().solve(Eigen::VectorXd::Ones(F.rows()));
      ++rc;
    }
  }

  A = A.topRows(rc);
  b = Eigen::VectorXd::Ones(A.rows());
  b = b + A * c.transpose();

  return true;
}
コード例 #2
0
ファイル: l2r_l1hinge_spdc.cpp プロジェクト: husk214/rp-velb
Eigen::VectorXd
l2r_l1hinge_spdc::convert_w2alpha(const Eigen::VectorXd &alp) const {
  Eigen::RowVectorXd w = Eigen::RowVectorXd::Zero(num_fea_);
  if (num_ins_ < alp.size()) {
    for (int i = 0; i < num_ins_; ++i)
      if (alp[i] > 0.0)
        w += (alp[i] * y_[i]) * x_.row(i);
  } else {
    for (int i = 0; i < alp.size(); ++i)
      if (alp[i] > 0.0)
        w += (alp[i] * y_[i]) * x_.row(i);
  }
  return w.transpose();
}