/**
* Generates an artificial topographyGrid of size numRows x numCols if no
* topographic data is available. Results are dumped into topographyGrid.
* @param topographyGrid A pointer to a zero-initialized Grid of size
* numRows x numCols.
* @param numRows The desired number of non-border rows in the resulting matrix.
* @param numCols The desired number of non-border cols in the resulting matrix.
*/
void simulatetopographyGrid(Grid* topographyGrid, int numRows, int numCols) {
Eigen::VectorXd refx = refx.LinSpaced(numCols, -2*M_PI, 2*M_PI);
Eigen::VectorXd refy = refx.LinSpaced(numRows, -2*M_PI, 2*M_PI);
Eigen::MatrixXd X = refx.replicate(1, numRows);
X.transposeInPlace();
Eigen::MatrixXd Y = refy.replicate(1, numCols);
// Eigen can only deal with two matrices at a time,
// so split the computation:
// topographyGrid = sin(X) * sin(Y) * abs(X) * abs(Y) -pi
Eigen::MatrixXd absXY = X.cwiseAbs().cwiseProduct(Y.cwiseAbs());
Eigen::MatrixXd sins = X.array().sin().cwiseProduct(Y.array().sin());
Eigen::MatrixXd temp;
temp.resize(numRows, numCols);
temp = absXY.cwiseProduct(sins);
// All this work to create a matrix of pi...
Eigen::MatrixXd pi;
pi.resize(numRows, numCols);
pi.setConstant(M_PI);
temp = temp - pi;
// And the final result.
topographyGrid->data.block(border, border, numRows, numCols) =
temp.block(0, 0, numRows, numCols);
// Ignore positive values.
topographyGrid->data =
topographyGrid->data.unaryExpr(std::ptr_fun(validateDepth));
topographyGrid->clearNA();
}
Eigen::MatrixXd ExperimentalTrajectory::rbfnKernelFunction(Eigen::VectorXd& evalVec)
{
int sizeVec = evalVec.rows();
int nC = kernelCenters.rows();
Eigen::MatrixXd centersMat = kernelCenters.transpose().replicate(sizeVec,1);
Eigen::MatrixXd evalMat = evalVec.replicate(1,nC);
return ((-(evalMat - centersMat).array().square()).array() / kernelLengthParameter).array().exp();
}
Eigen::MatrixXd sqExp(const Eigen::MatrixXd &x1, const Eigen::MatrixXd &x2,
const Eigen::VectorXd &lengthScale, bool noisy)
{
// assert(x1.rows() == x2.rows())
int n1 = x1.cols(), n2 = x2.cols();
// Compute the weighted square distances
Eigen::VectorXd w = (lengthScale.array().square().cwiseInverse()).matrix();
Eigen::MatrixXd xx1 = w.replicate(1, n1).cwiseProduct(x1).cwiseProduct(x1).colwise().sum();
Eigen::MatrixXd xx2 = w.replicate(1, n2).cwiseProduct(x2).cwiseProduct(x2).colwise().sum();
Eigen::MatrixXd x1x2 = w.replicate(1, n1).cwiseProduct(x1).transpose() * x2;
// Compute the covariance matrix
Eigen::MatrixXd K = (-0.5 *
Eigen::MatrixXd::Zero(n1, n2).cwiseMax(
xx1.transpose().replicate(1, n2) + xx2.replicate(n1, 1) - 2 * x1x2)).array().exp();
if (noisy) {
K += K.colwise().sum().asDiagonal();
}
return K;
}
Eigen::MatrixXd ExperimentalTrajectory::kernelFunction(double m)
{
int nC = kernelCenters.rows();
int nS = maxCovariance.rows();
Eigen::VectorXd kern = ((-(m - kernelCenters.array()).array().square()) / kernelLengthParameter).array().exp();
Eigen::MatrixXd result = (maxCovariance.transpose().replicate(nC, 1)).array() * kern.replicate(1, nS).array();
Eigen::MatrixXd blockDiagonalResult = Eigen::MatrixXd::Zero(nS, (nS*nC));
for(int i=0; i<result.rows(); i++)
{
blockDiagonalResult.block(0, i*nS, nS, nS) = result.row(i).asDiagonal();
}
return blockDiagonalResult;
}
