Beispiel #1
0
SEXP
GetMatCol(const SEXP data, const int idx)
{    
    Eigen::Ref<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> > A = EigenXPtrToMapEigen<T>(data);
    Eigen::Matrix<T, Eigen::Dynamic, 1> Am = A.col(idx-1);
    return(wrap(Am));
}
void FunctionApproximatorGPR::train(const Eigen::Ref<const Eigen::MatrixXd>& inputs, const Eigen::Ref<const Eigen::MatrixXd>& targets)
{
  if (isTrained())  
  {
    cerr << "WARNING: You may not call FunctionApproximatorGPR::train more than once. Doing nothing." << endl;
    cerr << "   (if you really want to retrain, call reTrain function instead)" << endl;
    return;
  }
  
  assert(inputs.rows() == targets.rows());
  assert(inputs.cols()==getExpectedInputDim());

  const MetaParametersGPR* meta_parameters_gpr = 
    dynamic_cast<const MetaParametersGPR*>(getMetaParameters());
  
  double max_covar = meta_parameters_gpr->maximum_covariance();
  VectorXd sigmas = meta_parameters_gpr->sigmas();
  
  
  // Compute the gram matrix
  // In a gram matrix, every input point is itself a center
  MatrixXd centers = inputs;
  // Replicate sigmas, because they are the same for each data point/center
  MatrixXd widths = sigmas.transpose().colwise().replicate(centers.rows()); 

  MatrixXd gram(inputs.rows(),inputs.rows());
  bool normalize_activations = false;
  bool asymmetric_kernels = false;
  BasisFunction::Gaussian::activations(centers,widths,inputs,gram,normalize_activations,asymmetric_kernels);
  
  gram *= max_covar;

  setModelParameters(new ModelParametersGPR(inputs,targets,gram,max_covar,sigmas));
  
}
Beispiel #3
0
Eigen::ArrayXXi distmesh::utils::findUniqueEdges(Eigen::Ref<Eigen::ArrayXXi const> const triangulation) {
    // find all unique combinations
    auto const combinations = nOverK(triangulation.cols(), 2);

    // find unique edges for all combinations
    // guarantee direction of edges with lower node index to higher index
    std::set<std::array<int, 2>> uniqueEdges;
    std::array<int, 2> edge = {{0, 0}};
    for (int combination = 0; combination < combinations.rows(); ++combination)
    for (int triangle = 0; triangle < triangulation.rows(); ++triangle) {
        edge[0] = triangulation(triangle, combinations(combination, 0));
        edge[1] = triangulation(triangle, combinations(combination, 1));

        edge = edge[1] < edge[0] ? std::array<int, 2>{edge[1], edge[0]} : edge;

        uniqueEdges.insert(edge);
    }

    // copy set to eigen array
    Eigen::ArrayXXi edgeIndices(uniqueEdges.size(), 2);
    int index = 0;
    for (auto const& edge : uniqueEdges) {
        edgeIndices(index, 0) = edge[0];
        edgeIndices(index, 1) = edge[1];

        index++;
    }

    return edgeIndices;
}
void FunctionApproximatorGPR::predictVariance(const Eigen::Ref<const Eigen::MatrixXd>& inputs, MatrixXd& variances)
{
  if (!isTrained())  
  {
    cerr << "WARNING: You may not call FunctionApproximatorLWPR::predict if you have not trained yet. Doing nothing." << endl;
    return;
  }

  const ModelParametersGPR* model_parameters_gpr = static_cast<const ModelParametersGPR*>(getModelParameters());
  
  
  assert(inputs.cols()==getExpectedInputDim());
  
  unsigned int n_samples = inputs.rows();
  variances.resize(n_samples,1);
  
  MatrixXd ks;
  model_parameters_gpr->kernelActivations(inputs, ks);  

  double maximum_covariance = model_parameters_gpr->maximum_covariance();
  MatrixXd gram_inv = model_parameters_gpr->gram_inv();
  
  for (unsigned int ii=0; ii<n_samples; ii++)
    variances(ii) = maximum_covariance - (ks.row(ii)*gram_inv).dot(ks.row(ii).transpose());

}
Beispiel #5
0
void LinearElasticIsotropic<DisplacementDim>::computeConstitutiveRelation(
        double const t,
        ProcessLib::SpatialPosition const& x,
        Eigen::Ref<Eigen::VectorXd const> w_prev,
        Eigen::Ref<Eigen::VectorXd const> w,
        Eigen::Ref<Eigen::VectorXd const> sigma_prev,
        Eigen::Ref<Eigen::VectorXd> sigma,
        Eigen::Ref<Eigen::MatrixXd> C,
        typename FractureModelBase<DisplacementDim>::MaterialStateVariables&
        material_state_variables)
{
    material_state_variables.reset();

    const int index_ns = DisplacementDim - 1;
    C.setZero();
    for (int i=0; i<index_ns; i++)
        C(i,i) = _mp.shear_stiffness(t, x)[0];
    C(index_ns, index_ns) = _mp.normal_stiffness(t, x)[0];

    sigma.noalias() = sigma_prev + C * (w - w_prev);

    // correct if fracture is opening
    if (sigma[index_ns] > 0)
    {
        C.setZero();
        sigma.setZero();
        material_state_variables.setTensileStress(true);
    }
}
double softmax<T>::compute_cost(const Eigen::Ref<const EigenMat> &train,
                                const Eigen::Ref<const EigenMat> &weight,
                                const Eigen::Ref<const EigenMat> &ground_truth)
{    
    compute_hypothesis(train, weight);
    double const NSamples = static_cast<double>(train.cols());
    return  -1.0 * (hypothesis_.array().log() *
                    ground_truth.array()).sum() / NSamples +
            weight.array().pow(2.0).sum() * params_.lambda_ / 2.0;
}
Beispiel #7
0
// create initial points distribution
Eigen::ArrayXXd distmesh::utils::createInitialPoints(
    Functional const& distanceFunction, double const initialPointDistance,
    Functional const& elementSizeFunction, Eigen::Ref<Eigen::ArrayXXd const> const boundingBox,
    Eigen::Ref<Eigen::ArrayXXd const> const fixedPoints) {
    // extract dimension of mesh
    unsigned const dimension = boundingBox.cols();

    // initially distribute points evenly in complete bounding box
    Eigen::ArrayXi pointsPerDimension(dimension);
    for (int dim = 0; dim < dimension; ++dim) {
        pointsPerDimension(dim) = ceil((boundingBox(1, dim) - boundingBox(0, dim)) /
            (initialPointDistance * (dim == 0 ? 1.0 : sqrt(3.0) / 2.0)));
    }

    Eigen::ArrayXXd points(pointsPerDimension.prod(), dimension);
    for (int point = 0; point < points.rows(); ++point)
    for (int dim = 0; dim < dimension; ++dim) {
        int const pointIndex = (point / std::max(pointsPerDimension.topRows(dim).prod(), 1)) %
            pointsPerDimension(dim);

        points(point, dim) = boundingBox(0, dim) + (double)pointIndex * initialPointDistance *
            (dim == 0 ? 1.0 : sqrt(3.0) / 2.0);

        if (dim > 0) {
            points(point, dim - 1) += pointIndex % 2 != 0 ? initialPointDistance / 2.0 : 0.0;
        }
    }

    // reject points outside of region defined by distance function
    points = selectMaskedArrayElements<double>(points,
        distanceFunction(points) < constants::geometryEvaluationThreshold * initialPointDistance);

    // clear duplicate points
    Eigen::Array<bool, Eigen::Dynamic, 1> isUniquePoint =
        Eigen::Array<bool, Eigen::Dynamic, 1>::Constant(points.rows(), true);
    for (int i = 0; i < fixedPoints.rows(); ++i)
    for (int j = 0; j < points.rows(); ++j) {
        isUniquePoint(j) &= !(fixedPoints.row(i) == points.row(j)).all();
    }
    points = selectMaskedArrayElements<double>(points, isUniquePoint);

    // calculate probability to keep points
    Eigen::ArrayXd probability = 1.0 / elementSizeFunction(points).pow(dimension);
    probability /= probability.maxCoeff();

    // reject points with wrong probability
    points = selectMaskedArrayElements<double>(points,
        0.5 * (1.0 + Eigen::ArrayXd::Random(points.rows())) < probability);

    // combine fixed and variable points to one array
    Eigen::ArrayXXd finalPoints(points.rows() + fixedPoints.rows(), dimension);
    finalPoints << fixedPoints, points;

    return finalPoints;
}
int softmax<T>::predict(Eigen::Ref<const EigenMat> const &input)
{    
    CV_Assert(input.cols() == 1);
    compute_hypothesis(input, weight_);
    probability_ = (hypothesis_ * input.transpose()).
            rowwise().sum();
    EigenMat::Index max_row = 0, max_col = 0;
    probability_.maxCoeff(&max_row, &max_col);

    return max_row;
}
void softmax<T>::compute_gradient(Eigen::Ref<const EigenMat> const &train,
                                  Eigen::Ref<const EigenMat> const &weight,
                                  Eigen::Ref<const EigenMat> const &ground_truth)
{
    grad_.noalias() =
            (ground_truth.array() - hypothesis_.array())
            .matrix() * train.transpose();
    auto const NSamples = static_cast<double>(train.cols());
    grad_.array() = grad_.array() / -NSamples +
            params_.lambda_ * weight.array();
}
Beispiel #10
0
// determine boundary edges of given triangulation
Eigen::ArrayXi distmesh::utils::boundEdges(
    Eigen::Ref<Eigen::ArrayXXi const> const triangulation,
    Eigen::Ref<Eigen::ArrayXXi const> const _edges,
    Eigen::Ref<Eigen::ArrayXXi const> const _edgeIndices) {
    // create a new edge list, if none was given
    Eigen::ArrayXXi edges;
    if (_edges.rows() == 0) {
        edges = utils::findUniqueEdges(triangulation);
    }
    else {
        edges = _edges;
    }

    // get edge indices for each triangle in triangulation
    Eigen::ArrayXXi edgeIndices;
    if (_edgeIndices.rows() == 0) {
        edgeIndices = utils::getTriangulationEdgeIndices(triangulation, edges);
    }
    else {
        edgeIndices = _edgeIndices;
    }

    // find edges, which only appear once in triangulation
    std::set<int> uniqueEdges;
    std::vector<int> boundaryEdges;
    for (int triangle = 0; triangle < triangulation.rows(); ++triangle)
    for (int edge = 0; edge < triangulation.cols(); ++edge) {
        auto const edgeIndex = edgeIndices(triangle, edge);

        // insert edge in set to get info about multiple appearance
        if (!std::get<1>(uniqueEdges.insert(edgeIndex))) {
            // find edge in vector and delete it
            auto const it = std::find(boundaryEdges.begin(), boundaryEdges.end(), edgeIndex);
            if (it != boundaryEdges.end()) {
                boundaryEdges.erase(it);
            }
        }
        else {
            boundaryEdges.push_back(edgeIndex);
        }
    }

    // convert stl vector to eigen array
    Eigen::ArrayXi boundary(boundaryEdges.size());
    for (int edge = 0; edge < boundary.rows(); ++edge) {
        boundary(edge) = boundaryEdges[edge];
    }
    
    return boundary;
}
//**************************************************************************************************
Eigen::MatrixRXd wholeBodyReach::pinvDampedEigen(const Eigen::Ref<Eigen::MatrixRXd> &A, double damp)
{
    // allocate memory
    int m = A.rows(), n = A.cols(), k = m<n?m:n;
    VectorXd SpinvD = VectorXd::Zero(k);
    // compute decomposition
    JacobiSVD<MatrixRXd> svd(A, ComputeThinU | ComputeThinV);    // default Eigen SVD
    VectorXd sv = svd.singularValues();
    // compute pseudoinverse of singular value matrix
    double damp2 = damp*damp;
    for (int c=0;c<k; c++)
        SpinvD(c) = sv(c) / (sv(c)*sv(c) + damp2);
    // compute damped pseudoinverse
    return svd.matrixV() * SpinvD.asDiagonal() * svd.matrixU().transpose();
}
void FunctionApproximatorGMR::predictVariance(const Eigen::Ref<const Eigen::MatrixXd>& inputs, Eigen::MatrixXd& variances)
{
  ENTERING_REAL_TIME_CRITICAL_CODE
  variances.resize(inputs.rows(),getExpectedOutputDim());
  predict(inputs,empty_prealloc_,variances);
  EXITING_REAL_TIME_CRITICAL_CODE
}
Beispiel #13
0
void DrawArrow(const Eigen::Ref<const Eigen::Vector3d>& pt, const Eigen::Ref<const Eigen::Vector3d>& dir, double length, double thickness, double arrowthickness) {
	Eigen::Vector3d normDir = dir.normalized();

	if(arrowthickness==-1) arrowthickness=2*thickness;
	double arrowlength = 2*arrowthickness;

	GLUquadricObj *c;
	c = gluNewQuadric();
	gluQuadricDrawStyle(c, GLU_FILL);
	gluQuadricNormals(c, GLU_SMOOTH);

	glPushMatrix();
	glTranslated(pt[0], pt[1], pt[2]);
	glRotated(acos(normDir[2])*180/M_PI, -normDir[1], normDir[0], 0);
	gluCylinder(c, thickness, thickness, length-arrowlength, 16, 16);

	// arrowhed
	glPushMatrix();
	glTranslated(0, 0, length-arrowlength);
	gluCylinder(c, arrowthickness, 0.0, arrowlength, 10, 10);
	glPopMatrix();

	glPopMatrix();

	gluDeleteQuadric(c);
}
Beispiel #14
0
// convert kartesian to polar coordinates
Eigen::ArrayXd mpFlow::math::polar(Eigen::Ref<Eigen::ArrayXd const> const point) {
    // calc radius
    double angle = 0.0;
    double radius = sqrt(point.square().sum());

    // calc angle
    if (point(0) > 0.0) {
        angle = atan(point(1) / point(0));
    }
    else if ((point(0) < 0.0) && (point(1) >= 0.0)) {
        angle = atan(point(1) / point(0)) + M_PI;
    }
    else if ((point(0) < 0.0) && (point(1) < 0.0)) {
        angle = atan(point(1) / point(0)) - M_PI;
    }
    else if ((point(0) == 0.0) && (point(1) > 0.0)) {
        angle = M_PI / 2.0;
    }
    else if ((point(0) == 0.0) && (point(1) < 0.0)) {
        angle = - M_PI / 2.0;
    }
    else {
        angle = 0.0;
    }

    Eigen::ArrayXd result(2);
    result << radius, angle;
    return result;
}
//**************************************************************************************************
Eigen::MatrixRXd wholeBodyReach::nullSpaceProjector(const Eigen::Ref<MatrixRXd> A, double tol)
{
    // allocate memory
    int m = A.rows(), n = A.cols(), k = m<n?m:n;
    MatrixRXd Spinv = MatrixRXd::Zero(k,k);
    // compute decomposition
    JacobiSVD<MatrixRXd> svd(A, ComputeThinU | ComputeThinV);    // default Eigen SVD
    VectorXd sv = svd.singularValues();
    // compute pseudoinverse of singular value matrix
    for (int c=0;c<k; c++)
        if ( sv(c)> tol)
            Spinv(c,c) = 1/sv(c);
    // compute pseudoinverse
    MatrixRXd N = MatrixRXd::Identity(n,n);
    N -= svd.matrixV() * Spinv  * svd.matrixU().transpose() * A;
    return N;
}
Beispiel #16
0
void assign (Eigen::Ref<Eigen::Vector3d> result, const geometry_msgs::Quaternion &q) {
	result << q.x, q.y, q.z;
	if (result.isMuchSmallerThan(1)) {
		result = Eigen::Vector3d::Zero();
	} else {
		double angle = 2. * acos(q.w);
		result *= angle / sin(0.5 * angle);
	}
}
Beispiel #17
0
Eigen::ArrayXXi distmesh::utils::getTriangulationEdgeIndices(
    Eigen::Ref<Eigen::ArrayXXi const> const triangulation,
    Eigen::Ref<Eigen::ArrayXXi const> const edges) {
    // find indices for each edge of triangulation in edge index array
    Eigen::ArrayXXi edgeIndices(triangulation.rows(), triangulation.cols());
    for (int element = 0; element < triangulation.rows(); ++element)
    for (int node = 0; node < triangulation.cols(); ++node) {
        // create edge with direction from node with lower index
        // to node with higher index
        auto const edge = (Eigen::ArrayXi(2) << triangulation(element, node), triangulation(element, (node + 1) % triangulation.cols())).finished();

        // check if edge is in edges list, and get index
        int edgeIndex = 0;
        if (((edges.rowwise() - edge.transpose()).square().rowwise().sum().minCoeff(&edgeIndex) == 0) ||
            ((edges.rowwise() - edge.transpose().reverse()).square().rowwise().sum().minCoeff(&edgeIndex) == 0)) {
            edgeIndices(element, node) = edgeIndex;
        }
    }

    return edgeIndices;
}
Beispiel #18
0
void softmax<T>::train(const Eigen::Ref<const EigenMat> &train,
                       const std::vector<int> &labels)
{
#ifdef OCV_TEST_SOFTMAX
    gradient_check();
#endif

    auto const UniqueLabels = get_unique_labels(labels);
    auto const NumClass = UniqueLabels.size();
    weight_ = EigenMat::Random(NumClass, train.rows());
    grad_ = EigenMat::Zero(NumClass, train.rows());
    auto const TrainCols = static_cast<int>(train.cols());
    EigenMat const GroundTruth = get_ground_truth(static_cast<int>(NumClass),
                                                  TrainCols,
                                                  UniqueLabels,
                                                  labels);

    std::random_device rd;
    std::default_random_engine re(rd());
    int const Batch = (get_batch_size(TrainCols));
    int const RandomSize = TrainCols != Batch ?
                TrainCols - Batch - 1 : 0;
    std::uniform_int_distribution<int>
            uni_int(0, RandomSize);
    for(size_t i = 0; i != params_.max_iter_; ++i){
        auto const Cols = uni_int(re);
        auto const &TrainBlock =
                train.block(0, Cols, train.rows(), Batch);
        auto const &GTBlock =
                GroundTruth.block(0, Cols, NumClass, Batch);
        auto const Cost = compute_cost(TrainBlock, weight_, GTBlock);
        if(std::abs(params_.cost_ - Cost) < params_.epsillon_ ||
                Cost < 0){
            break;
        }
        params_.cost_ = Cost;
        compute_gradient(TrainBlock, weight_, GTBlock);
        weight_.array() -= grad_.array() * params_.lrate_;//*/
    }
}
Beispiel #19
0
// check whether points lies inside or outside of polygon
Eigen::ArrayXd distmesh::utils::pointsInsidePoly(
    Eigen::Ref<Eigen::ArrayXXd const> const points,
    Eigen::Ref<Eigen::ArrayXXd const> const polygon) {
    Eigen::ArrayXd inside = Eigen::ArrayXd::Zero(points.rows());

    for (int i = 0, j = polygon.rows() - 1; i < polygon.rows(); j = i++) {
        inside = (((points.col(1) < polygon(i, 1)) != (points.col(1) < polygon(j, 1))) &&
            (points.col(0) < (polygon(j, 0) - polygon(i, 0)) * (points.col(1) - polygon(i, 1)) /
            (polygon(j, 1) - polygon(i, 1)) + polygon(i, 0))).select(1.0 - inside, inside);
    }

    return inside;
}
void FunctionApproximatorGPR::predict(const Eigen::Ref<const Eigen::MatrixXd>& inputs, MatrixXd& outputs)
{
  if (!isTrained())  
  {
    cerr << "WARNING: You may not call FunctionApproximatorLWPR::predict if you have not trained yet. Doing nothing." << endl;
    return;
  }

  const ModelParametersGPR* model_parameters_gpr = static_cast<const ModelParametersGPR*>(getModelParameters());
  
  assert(inputs.cols()==getExpectedInputDim());
  unsigned int n_samples = inputs.rows();
  
  outputs.resize(n_samples,1);
  
  MatrixXd ks(n_samples,n_samples);
  model_parameters_gpr->kernelActivations(inputs, ks);
  
  
  VectorXd weights = model_parameters_gpr->weights();
  for (unsigned int ii=0; ii<n_samples; ii++)
    outputs(ii) = ks.row(ii).dot(weights);
  
}
void LinearElasticIsotropic<DisplacementDim>::computeConstitutiveRelation(
    double const t,
    ProcessLib::SpatialPosition const& x,
    double const aperture0,
    Eigen::Ref<Eigen::VectorXd const>
        sigma0,
    Eigen::Ref<Eigen::VectorXd const>
    /*w_prev*/,
    Eigen::Ref<Eigen::VectorXd const>
        w,
    Eigen::Ref<Eigen::VectorXd const>
    /*sigma_prev*/,
    Eigen::Ref<Eigen::VectorXd>
        sigma,
    Eigen::Ref<Eigen::MatrixXd>
        C,
    typename FractureModelBase<DisplacementDim>::MaterialStateVariables&
        material_state_variables)
{
    material_state_variables.reset();

    const int index_ns = DisplacementDim - 1;
    C.setZero();
    for (int i = 0; i < index_ns; i++)
        C(i, i) = _mp.shear_stiffness(t, x)[0];

    sigma.noalias() = C * w;

    double const aperture = w[index_ns] + aperture0;

    sigma.coeffRef(index_ns) =
        _mp.normal_stiffness(t, x)[0] * w[index_ns] *
        logPenalty(aperture0, aperture, _penalty_aperture_cutoff);

    C(index_ns, index_ns) =
        _mp.normal_stiffness(t, x)[0] *
        logPenaltyDerivative(aperture0, aperture, _penalty_aperture_cutoff);

    sigma.noalias() += sigma0;

    // correction for an opening fracture
    if (_tension_cutoff && sigma[index_ns] > 0)
    {
        C.setZero();
        sigma.setZero();
        material_state_variables.setTensileStress(true);
    }
}
Beispiel #22
0
    /** Draw shape triangles */
    void drawShapeTriangulation(cv::Mat& canvas, Eigen::Ref<RowVectorX const> shape, Eigen::Ref<RowVectorXi const> triangleIds, const cv::Scalar &color)
    {
        const auto &s = shape.cast<float>();
        
        for (int j = 0; j < triangleIds.cols() / 3; j++) {

            int id1 = triangleIds(0, j * 3 + 0);
            int id2 = triangleIds(0, j * 3 + 1);
            int id3 = triangleIds(0, j * 3 + 2);

            float x1 = s(0, id1 * 2 + 0);
            float y1 = s(0, id1 * 2 + 1);

            float x2 = s(0, id2 * 2 + 0);
            float y2 = s(0, id2 * 2 + 1);

            float x3 = s(0, id3 * 2 + 0);
            float y3 = s(0, id3 * 2 + 1);

            cv::line(canvas, cv::Point2f(x1, y1), cv::Point2f(x2, y2), color, 1, CV_AA);
            cv::line(canvas, cv::Point2f(x2, y2), cv::Point2f(x3, y3), color, 1, CV_AA);
            cv::line(canvas, cv::Point2f(x3, y3), cv::Point2f(x1, y1), color, 1, CV_AA);
        }
    }
    void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const override
    {
        const unsigned int offset = 3 * links_ * chainNum_;
        out.setZero();

        Eigen::VectorXd plus(3 * (links_ + 1));
        plus.head(3 * links_) = x.segment(offset, 3 * links_);
        plus.tail(3) = Eigen::VectorXd::Zero(3);

        Eigen::VectorXd minus(3 * (links_ + 1));
        minus.head(3) = offset_;
        minus.tail(3 * links_) = x.segment(offset, 3 * links_);

        const Eigen::VectorXd diagonal = plus - minus;

        for (unsigned int i = 0; i < links_; i++)
            out.row(i).segment(3 * i + offset, 3) = diagonal.segment(3 * i, 3).normalized();

        out.block(1, offset, links_ - 1, 3 * links_ - 3) -= out.block(1, offset + 3, links_ - 1, 3 * links_ - 3);
    }
Beispiel #24
0
// fix orientation of edges located at the boundary
Eigen::ArrayXXi distmesh::utils::fixBoundaryEdgeOrientation(
    Eigen::Ref<Eigen::ArrayXXd const> const nodes,
    Eigen::Ref<Eigen::ArrayXXi const> const triangulation,
    Eigen::Ref<Eigen::ArrayXXi const> const _edges,
    Eigen::Ref<Eigen::ArrayXXi const> const edgeIndices) {
    Eigen::ArrayXXi edges = _edges;

    // for the 2-D case fix orientation of boundary edges
    if (nodes.cols() == 2) {
        auto const boundary = utils::boundEdges(triangulation, edges, edgeIndices);

        for (int edge = 0; edge < boundary.rows(); ++edge) {
            // find get index of element containing boundary edge
            int elementIndex = 0, edgeIndex = 0;
            (edgeIndices - boundary(edge)).square().minCoeff(&elementIndex, &edgeIndex);

            // get index of node not used in edge, but in the triangle
            int nodeIndex = 0;
            for (int node = 0; node < triangulation.cols(); ++node) {
                if ((triangulation(elementIndex, node) != edges(boundary(edge), 0)) &&
                    (triangulation(elementIndex, node) != edges(boundary(edge), 1))) {
                    nodeIndex = node;
                    break;
                }
            }

            // boundary edges with wrong orientation are marked with a negative sign
            auto const v1 = (nodes.row(edges(boundary(edge), 1)) - nodes.row(edges(boundary(edge), 0))).eval();
            auto const v2 = (nodes.row(triangulation(elementIndex, nodeIndex)) - nodes.row(edges(boundary(edge), 1))).eval();
            if (v1(0) * v2(1) - v1(1) * v2(0) < 0.0) {
                edges.row(boundary(edge)) = edges.row(boundary(edge)).reverse().eval();
            }
        }
    }

    return edges;
}
Beispiel #25
0
// project points outside of domain back to boundary
void distmesh::utils::projectPointsToBoundary(
    Functional const& distanceFunction, double const initialPointDistance,
    Eigen::Ref<Eigen::ArrayXXd> points) {
    Eigen::ArrayXd distance = distanceFunction(points);

    // check for points outside of boundary
    Eigen::Array<bool, Eigen::Dynamic, 1> outside = distance > 0.0;
    if (outside.any()) {
        // calculate gradient
        Eigen::ArrayXXd gradient(points.rows(), points.cols());
        Eigen::ArrayXXd deltaX = Eigen::ArrayXXd::Zero(points.rows(), points.cols());

        for (int dim = 0; dim < points.cols(); ++dim) {
            deltaX.col(dim).fill(constants::deltaX * initialPointDistance);
            gradient.col(dim) = (distanceFunction(points + deltaX) - distance) /
                (constants::deltaX * initialPointDistance);
            deltaX.col(dim).fill(0.0);
        }

        // project points back to boundary
        points -= outside.replicate(1, points.cols()).select(
            gradient.colwise() * distance / gradient.square().rowwise().sum(), 0.0);
    }
}
Beispiel #26
0
void MohrCoulomb<DisplacementDim>::computeConstitutiveRelation(
    double const t,
    ProcessLib::SpatialPosition const& x,
    double const aperture0,
    Eigen::Ref<Eigen::VectorXd const>
        sigma0,
    Eigen::Ref<Eigen::VectorXd const>
        w_prev,
    Eigen::Ref<Eigen::VectorXd const>
        w,
    Eigen::Ref<Eigen::VectorXd const>
        sigma_prev,
    Eigen::Ref<Eigen::VectorXd>
        sigma,
    Eigen::Ref<Eigen::MatrixXd>
        Kep,
    typename FractureModelBase<DisplacementDim>::MaterialStateVariables&
        material_state_variables)
{
    material_state_variables.reset();

    MaterialPropertyValues const mat(_mp, t, x);
    Eigen::VectorXd const dw = w - w_prev;

    const int index_ns = DisplacementDim - 1;
    double const aperture = w[index_ns] + aperture0;
    double const aperture_prev = w_prev[index_ns] + aperture0;

    Eigen::MatrixXd Ke;
    {  // Elastic tangent stiffness
        Ke = Eigen::MatrixXd::Zero(DisplacementDim, DisplacementDim);
        for (int i = 0; i < index_ns; i++)
            Ke(i, i) = mat.Ks;

        Ke(index_ns, index_ns) =
            mat.Kn *
            logPenaltyDerivative(aperture0, aperture, _penalty_aperture_cutoff);
    }

    Eigen::MatrixXd Ke_prev;
    {  // Elastic tangent stiffness at w_prev
        Ke_prev = Eigen::MatrixXd::Zero(DisplacementDim, DisplacementDim);
        for (int i = 0; i < index_ns; i++)
            Ke_prev(i, i) = mat.Ks;

        Ke_prev(index_ns, index_ns) =
            mat.Kn * logPenaltyDerivative(
                         aperture0, aperture_prev, _penalty_aperture_cutoff);
    }

    // Total plastic aperture compression
    // NOTE: Initial condition sigma0 seems to be associated with an initial
    // condition of the w0 = 0. Therefore the initial state is not associated
    // with a plastic aperture change.
    Eigen::VectorXd const w_p_prev =
        w_prev - Ke_prev.fullPivLu().solve(sigma_prev - sigma0);

    {  // Exact elastic predictor
        sigma.noalias() = Ke * (w - w_p_prev);

        sigma.coeffRef(index_ns) =
            mat.Kn * w[index_ns] *
            logPenalty(aperture0, aperture, _penalty_aperture_cutoff);
    }

    sigma.noalias() += sigma0;

    double const sigma_n = sigma[index_ns];

    // correction for an opening fracture
    if (_tension_cutoff && sigma_n > 0)
    {
        Kep.setZero();
        sigma.setZero();
        material_state_variables.setTensileStress(true);
        return;
    }

    // check shear yield function (Fs)
    Eigen::VectorXd const sigma_s = sigma.head(DisplacementDim-1);
    double const mag_tau = sigma_s.norm(); // magnitude
    double const Fs = mag_tau + sigma_n * std::tan(mat.phi) - mat.c;

    material_state_variables.setShearYieldFunctionValue(Fs);
    if (Fs < .0)
    {
        Kep = Ke;
        return;
    }

    Eigen::VectorXd dFs_dS(DisplacementDim);
    dFs_dS.head(DisplacementDim-1).noalias() = sigma_s.normalized();
    dFs_dS[index_ns] = std::tan(mat.phi);

    // plastic potential function: Qs = |tau| + Sn * tan da
    Eigen::VectorXd dQs_dS = dFs_dS;
    dQs_dS[index_ns] = std::tan(mat.psi);

    // plastic multiplier
    Eigen::RowVectorXd const A = dFs_dS.transpose() * Ke / (dFs_dS.transpose() * Ke * dQs_dS);
    double const d_eta = A * dw;

    // plastic part of the dispalcement
    Eigen::VectorXd const dwp = dQs_dS * d_eta;

    // correct stress
    sigma.noalias() = sigma_prev + Ke * (dw - dwp);

    // Kep
    Kep = Ke - Ke * dQs_dS * A;
}
Beispiel #27
0
double Nullspace
(
  const Eigen::Ref<const Mat> & A,
  Eigen::Ref<Vec> nullspace
)
{
  if ( A.rows() >= A.cols() )
  {
    Eigen::JacobiSVD<Mat> svd( A, Eigen::ComputeFullV );
    nullspace = svd.matrixV().col( A.cols() - 1 );
    return svd.singularValues()( A.cols() - 1 );
  }
  // Extend A with rows of zeros to make it square. It's a hack, but it is
  // necessary until Eigen supports SVD with more columns than rows.
  Mat A_extended( A.cols(), A.cols() );
  A_extended.block( A.rows(), 0, A.cols() - A.rows(), A.cols() ).setZero();
  A_extended.block( 0, 0, A.rows(), A.cols() ) = A;
  return Nullspace( A_extended, nullspace );
}
void FunctionApproximatorGMR::train(const Eigen::Ref<const Eigen::MatrixXd>& inputs, const Eigen::Ref<const Eigen::MatrixXd>& targets)
{
  if (isTrained())  
  {
    cerr << "WARNING: You may not call FunctionApproximatorGMR::train more than once. Doing nothing." << endl;
    cerr << "   (if you really want to retrain, call reTrain function instead)" << endl;
    return;
  }
  
  assert(inputs.rows() == targets.rows()); // Must have same number of examples
  assert(inputs.cols() == getExpectedInputDim());

  const MetaParametersGMR* meta_parameters_GMR = 
    static_cast<const MetaParametersGMR*>(getMetaParameters());

  const ModelParametersGMR* model_parameters_GMR =
    static_cast<const ModelParametersGMR*>(getModelParameters());

  int n_gaussians;
  if(meta_parameters_GMR!=NULL)
      n_gaussians = meta_parameters_GMR->number_of_gaussians_;
  else if(model_parameters_GMR!=NULL)
      n_gaussians = model_parameters_GMR->priors_.size();
  else
      cerr << "FunctionApproximatorGMR::train Something wrong happened, both ModelParameters and MetaParameters are not initialized." << endl;

  int n_dims_in = inputs.cols();
  int n_dims_out = targets.cols();
  int n_dims_gmm = n_dims_in + n_dims_out;
  
  // Initialize the means, priors and covars
  std::vector<VectorXd> means(n_gaussians);
  std::vector<MatrixXd> covars(n_gaussians);
  std::vector<double> priors(n_gaussians);
  int n_observations = 0;

  for (int i = 0; i < n_gaussians; i++)
  {
    means[i] = VectorXd(n_dims_gmm);
    priors[i] = 0.0;
    covars[i] = MatrixXd(n_dims_gmm, n_dims_gmm);
  }
  
  // Put the input/output data in one big matrix
  MatrixXd data = MatrixXd(inputs.rows(), n_dims_gmm);
  data << inputs, targets;
  n_observations = data.rows();

  // Initialization
  if (inputs.cols() == 1)
    firstDimSlicingInit(data, means, priors, covars);
  else
    kMeansInit(data, means, priors, covars);
  
  // Expectation-Maximization
  expectationMaximization(data, means, priors, covars);

  // Extract the different input/output components from the means/covars which contain both
  std::vector<Eigen::VectorXd> means_x(n_gaussians);
  std::vector<Eigen::VectorXd> means_y(n_gaussians);
  std::vector<Eigen::MatrixXd> covars_x(n_gaussians);
  std::vector<Eigen::MatrixXd> covars_y(n_gaussians);
  std::vector<Eigen::MatrixXd> covars_y_x(n_gaussians);
  for (int i_gau = 0; i_gau < n_gaussians; i_gau++)
  {
    means_x[i_gau]    = means[i_gau].segment(0, n_dims_in);
    means_y[i_gau]    = means[i_gau].segment(n_dims_in, n_dims_out);

    covars_x[i_gau]   = covars[i_gau].block(0, 0, n_dims_in, n_dims_in);
    covars_y[i_gau]   = covars[i_gau].block(n_dims_in, n_dims_in, n_dims_out, n_dims_out);
    covars_y_x[i_gau] = covars[i_gau].block(n_dims_in, 0, n_dims_out, n_dims_in);
  }

  setModelParameters(new ModelParametersGMR(n_observations, priors, means_x, means_y, covars_x, covars_y, covars_y_x));

  // After training, we know the sizes of the matrices that should be cached
  preallocateMatrices(n_gaussians,n_dims_in,n_dims_out);
  
  // std::vector<VectorXd> centers;
  // std::vector<MatrixXd> slopes;
  // std::vector<VectorXd> biases;
  // std::vector<MatrixXd> inverseCovarsL;

  // // int n_dims_in = inputs.cols();
  // // int n_dims_out = targets.cols();

  // for (int i_gau = 0; i_gau < n_gaussians; i_gau++)
  // {
  //   centers.push_back(VectorXd(means[i_gau].segment(0, n_dims_in)));

  //   slopes.push_back(MatrixXd(covars[i_gau].block(n_dims_in, 0, n_dims_out, n_dims_in) * covars[i_gau].block(0, 0, n_dims_in, n_dims_in).inverse()));
    
  //   biases.push_back(VectorXd(means[i_gau].segment(n_dims_in, n_dims_out) -
  //     slopes[i_gau]*means[i_gau].segment(0, n_dims_in)));

  //   MatrixXd L = covars[i_gau].block(0, 0, n_dims_in, n_dims_in).inverse().llt().matrixL();
  //   inverseCovarsL.push_back(MatrixXd(L));
  // }

  // setModelParameters(new ModelParametersGMR(centers, priors, slopes, biases, inverseCovarsL));

  //for (size_t i = 0; i < means.size(); i++)
  //  delete means[i];
  //for (size_t i = 0; i < covars.size(); i++)
  //delete covars[i];
}
/** \todo Document FunctionApproximatorGMR::trainIncremental 
 */
void FunctionApproximatorGMR::trainIncremental(const Eigen::Ref<const Eigen::MatrixXd>& inputs, const Eigen::Ref<const Eigen::MatrixXd>& targets)
{
  if (!isTrained())
  {
    //cout << " Training for the first time... " << endl;
    train(inputs,targets);
    return;
  }

  const ModelParametersGMR* model_parameters_GMR = static_cast<const ModelParametersGMR*>(getModelParameters());


  int n_gaussians = model_parameters_GMR->priors_.size();
  int n_dims_in = inputs.cols();
  int n_dims_out = targets.cols();
  int n_dims_gmm = n_dims_in + n_dims_out;

  // Initialize the means, priors and covars
  std::vector<VectorXd> means(n_gaussians);
  std::vector<MatrixXd> covars(n_gaussians);
  std::vector<double> priors(n_gaussians);
  int n_observations = 0;
  for (int i = 0; i < n_gaussians; i++)
  {
    means[i] = VectorXd(n_dims_gmm);
    priors[i] = 0.0;
    covars[i] = MatrixXd(n_dims_gmm, n_dims_gmm);
  }

  // Extract the model parameters
  for (int i = 0; i < n_gaussians; i++)
  {
    means[i].segment(0, n_dims_in)    = model_parameters_GMR->means_x_[i];
    means[i].segment(n_dims_in, n_dims_out)    = model_parameters_GMR->means_y_[i];

    covars[i].block(0, 0, n_dims_in, n_dims_in)   = model_parameters_GMR->covars_x_[i];
    covars[i].block(n_dims_in, n_dims_in, n_dims_out, n_dims_out)   = model_parameters_GMR->covars_y_[i];
    covars[i].block(n_dims_in, 0, n_dims_out, n_dims_in) = model_parameters_GMR->covars_y_x_[i];

    priors[i] = model_parameters_GMR->priors_[i];
  }
  n_observations = model_parameters_GMR->n_observations_;

  // Put the input/output data in one big matrix
  MatrixXd data = MatrixXd(inputs.rows(), n_dims_gmm);
  data << inputs, targets;

  // Expectation-Maximization Incremental
  expectationMaximizationIncremental(data, means, priors, covars, n_observations);

  // Extract the different input/output components from the means/covars which contain both
  std::vector<Eigen::VectorXd> means_x(n_gaussians);
  std::vector<Eigen::VectorXd> means_y(n_gaussians);
  std::vector<Eigen::MatrixXd> covars_x(n_gaussians);
  std::vector<Eigen::MatrixXd> covars_y(n_gaussians);
  std::vector<Eigen::MatrixXd> covars_y_x(n_gaussians);
  for (int i_gau = 0; i_gau < n_gaussians; i_gau++)
  {
    means_x[i_gau]    = means[i_gau].segment(0, n_dims_in);
    means_y[i_gau]    = means[i_gau].segment(n_dims_in, n_dims_out);

    covars_x[i_gau]   = covars[i_gau].block(0, 0, n_dims_in, n_dims_in);
    covars_y[i_gau]   = covars[i_gau].block(n_dims_in, n_dims_in, n_dims_out, n_dims_out);
    covars_y_x[i_gau] = covars[i_gau].block(n_dims_in, 0, n_dims_out, n_dims_in);
  }

  setModelParameters(new ModelParametersGMR(n_observations, priors, means_x, means_y, covars_x, covars_y, covars_y_x));

  // After training, we know the sizes of the matrices that should be cached
  preallocateMatrices(n_gaussians,n_dims_in,n_dims_out);
}
Beispiel #30
0
void
SetMatRow(SEXP data, const int idx, SEXP value)
{    
    Eigen::Ref<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> > A = EigenXPtrToMapEigen<T>(data);
    A.row(idx-1) = as<Eigen::Matrix<T, Eigen::Dynamic, 1> >(value);
}