예제 #1
0
/**
 * 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();
}
예제 #3
0
파일: kernel.cpp 프로젝트: NICTA/obsidian
    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;

}