void PolygonIterator::findSubmapParameters(const grid_map::Polygon& polygon, Eigen::Array2i& startIndex, Eigen::Array2i& bufferSize) const
{
Eigen::Vector2d topLeft = polygon_.getVertices()[0];
Eigen::Vector2d bottomRight = topLeft;
for (const auto& vertex : polygon_.getVertices()) {
topLeft = topLeft.array().max(vertex.array());
bottomRight = bottomRight.array().min(vertex.array());
}
limitPositionToRange(topLeft, mapLength_, mapPosition_);
limitPositionToRange(bottomRight, mapLength_, mapPosition_);
getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
Eigen::Array2i endIndex;
getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
bufferSize = endIndex - startIndex + Eigen::Array2i::Ones();
}
Eigen::MatrixXd sqExp2d(const Eigen::Matrix<double, 2, -1> &x1,
const Eigen::Matrix<double, 2, -1> &x2,
const Eigen::Vector2d &lengthScale, bool noisy)
{
// assert(x1.rows() == x2.rows())
int n1 = x1.cols(), n2 = x2.cols();
// Compute the weighted square distances
Eigen::Vector2d w = (lengthScale.array().square().cwiseInverse()).matrix();
Eigen::RowVectorXd xx1 = w.replicate(1, n1).cwiseProduct(x1).cwiseProduct(x1).colwise().sum();
Eigen::RowVectorXd 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;
}