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));
}
// 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;
}
bool mrpt::vision::pnp::CPnP::so3(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat)
{
try{
// Input 2d/3d correspondences and camera intrinsic matrix
Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;
// Check for consistency of input matrix dimensions
if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
throw(2);
else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
throw(3);
if(obj_pts.rows() < obj_pts.cols())
{
cam_in_eig=cam_intrinsic.transpose();
img_pts_eig=img_pts.transpose().block(0,0,n,2);
obj_pts_eig=obj_pts.transpose();
}
else
{
cam_in_eig=cam_intrinsic;
img_pts_eig=img_pts.block(0,0,n,2);
obj_pts_eig=obj_pts;
}
// Output pose
Eigen::Matrix3d R;
Eigen::Vector3d t;
// Compute pose
mrpt::vision::pnp::p3p p(cam_in_eig);
p.solve(R,t, obj_pts_eig, img_pts_eig);
mrpt::vision::pnp::so3 s(obj_pts_eig, img_pts_eig, cam_in_eig, n);
bool ret = s.compute_pose(R,t);
Eigen::Quaterniond q(R);
pose_mat<<t,q.vec();
return ret;
}
catch(int e)
{
switch(e)
{
case 2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
case 3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
}
return false;
}
}
// 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;
}
// 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;
}
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());
}
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 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
}
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 );
}
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_;//*/
}
}
//**************************************************************************************************
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();
}
// 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);
}
}
//**************************************************************************************************
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 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 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];
}
bool mrpt::vision::pnp::CPnP::dls(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat){
try{
#if MRPT_HAS_OPENCV==1
// Input 2d/3d correspondences and camera intrinsic matrix
Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;
// Check for consistency of input matrix dimensions
if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
throw(2);
else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
throw(3);
if(obj_pts.rows() < obj_pts.cols())
{
cam_in_eig=cam_intrinsic.transpose();
img_pts_eig=img_pts.transpose().block(0,0,n,2);
obj_pts_eig=obj_pts.transpose();
}
else
{
cam_in_eig=cam_intrinsic;
img_pts_eig=img_pts.block(0,0,n,2);
obj_pts_eig=obj_pts;
}
// Output pose
Eigen::Matrix3d R_eig;
Eigen::MatrixXd t_eig;
// Compute pose
cv::Mat cam_in_cv(3,3,CV_32F), img_pts_cv(2,n,CV_32F), obj_pts_cv(3,n,CV_32F), R_cv(3,3,CV_32F), t_cv(3,1,CV_32F);
cv::eigen2cv(cam_in_eig, cam_in_cv);
cv::eigen2cv(img_pts_eig, img_pts_cv);
cv::eigen2cv(obj_pts_eig, obj_pts_cv);
mrpt::vision::pnp::dls d(obj_pts_cv, img_pts_cv);
bool ret = d.compute_pose(R_cv,t_cv);
cv::cv2eigen(R_cv, R_eig);
cv::cv2eigen(t_cv, t_eig);
Eigen::Quaterniond q(R_eig);
pose_mat << t_eig,q.vec();
return ret;
#else
throw(-1)
#endif
}
catch(int e)
{
switch(e)
{
case -1: std::cout << "Please install OpenCV for DLS-PnP" << std::endl;
case 2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
case 3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
}
return false;
}
}
/** \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);
}
// apply the distmesh algorithm
std::tuple<Eigen::ArrayXXd, Eigen::ArrayXXi> distmesh::distmesh(
Functional const& distanceFunction, double const initialPointDistance,
Functional const& elementSizeFunction, Eigen::Ref<Eigen::ArrayXXd const> const boundingBox,
Eigen::Ref<Eigen::ArrayXXd const> const fixedPoints) {
// determine dimension of mesh
unsigned const dimension = boundingBox.cols();
// create initial distribution in bounding box
Eigen::ArrayXXd points = utils::createInitialPoints(distanceFunction,
initialPointDistance, elementSizeFunction, boundingBox, fixedPoints);
// create initial triangulation
Eigen::ArrayXXi triangulation = triangulation::delaunay(points);
// create buffer to store old point locations to calculate
// retriangulation and stop criterion
Eigen::ArrayXXd retriangulationCriterionBuffer = Eigen::ArrayXXd::Constant(
points.rows(), points.cols(), INFINITY);
Eigen::ArrayXXd stopCriterionBuffer = Eigen::ArrayXXd::Zero(
points.rows(), points.cols());
// main distmesh loop
Eigen::ArrayXXi edgeIndices;
for (unsigned step = 0; step < constants::maxSteps; ++step) {
// retriangulate if point movement is above threshold
if ((points - retriangulationCriterionBuffer).square().rowwise().sum().sqrt().maxCoeff() >
constants::retriangulationThreshold * initialPointDistance) {
// update triangulation
triangulation = triangulation::delaunay(points);
// reject triangles with circumcenter outside of the region
Eigen::ArrayXXd circumcenter = Eigen::ArrayXXd::Zero(triangulation.rows(), dimension);
for (int point = 0; point < triangulation.cols(); ++point) {
circumcenter += utils::selectIndexedArrayElements<double>(
points, triangulation.col(point)) / triangulation.cols();
}
triangulation = utils::selectMaskedArrayElements<int>(triangulation,
distanceFunction(circumcenter) < -constants::geometryEvaluationThreshold * initialPointDistance);
// find unique edge indices
edgeIndices = utils::findUniqueEdges(triangulation);
// store current points positions
retriangulationCriterionBuffer = points;
}
// calculate edge vectors and their length
auto const edgeVector = (utils::selectIndexedArrayElements<double>(points, edgeIndices.col(0)) -
utils::selectIndexedArrayElements<double>(points, edgeIndices.col(1))).eval();
auto const edgeLength = edgeVector.square().rowwise().sum().sqrt().eval();
// evaluate elementSizeFunction at midpoints of edges
auto const desiredElementSize = elementSizeFunction(0.5 *
(utils::selectIndexedArrayElements<double>(points, edgeIndices.col(0)) +
utils::selectIndexedArrayElements<double>(points, edgeIndices.col(1)))).eval();
// calculate desired edge length
auto const desiredEdgeLength = (desiredElementSize * (1.0 + 0.4 / std::pow(2.0, dimension - 1)) *
std::pow((edgeLength.pow(dimension).sum() / desiredElementSize.pow(dimension).sum()),
1.0 / dimension)).eval();
// calculate force vector for each edge
auto const forceVector = (edgeVector.colwise() *
((desiredEdgeLength - edgeLength) / edgeLength).max(0.0)).eval();
// store current points positions
stopCriterionBuffer = points;
// move points
for (int edge = 0; edge < edgeIndices.rows(); ++edge) {
if (edgeIndices(edge, 0) >= fixedPoints.rows()) {
points.row(edgeIndices(edge, 0)) += constants::deltaT * forceVector.row(edge);
}
if (edgeIndices(edge, 1) >= fixedPoints.rows()) {
points.row(edgeIndices(edge, 1)) -= constants::deltaT * forceVector.row(edge);
}
}
// project points outside of domain to boundary
utils::projectPointsToBoundary(distanceFunction, initialPointDistance, points);
// stop, when maximum points movement is below threshold
if ((points - stopCriterionBuffer).square().rowwise().sum().sqrt().maxCoeff() <
constants::pointsMovementThreshold * initialPointDistance) {
break;
}
}
return std::make_tuple(points, triangulation);
}
Polynomial<CoefficientType>::Polynomial(Eigen::Ref<CoefficientsType> const& coefficients) :
coefficients(coefficients)
{
assert(coefficients.rows() > 0);
}
