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)); }
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()); }
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; }
// 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(); }
// 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 }
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); }
// 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; }
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); } }
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; }
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 >Block = 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_;//*/ } }
// 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); } }
/** 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); }
// 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; }
// 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); } }
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; }
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); }
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); }