| 269 | } |
| 270 | |
| 271 | bool getSubmapInformation(Index& submapTopLeftIndex, |
| 272 | Size& submapBufferSize, |
| 273 | Position& submapPosition, |
| 274 | Length& submapLength, |
| 275 | Index& requestedIndexInSubmap, |
| 276 | const Position& requestedSubmapPosition, |
| 277 | const Length& requestedSubmapLength, |
| 278 | const Length& mapLength, |
| 279 | const Position& mapPosition, |
| 280 | const double& resolution, |
| 281 | const Size& bufferSize, |
| 282 | const Index& bufferStartIndex) |
| 283 | { |
| 284 | // (Top left / bottom right corresponds to the position in the matrix, not the map frame) |
| 285 | const Eigen::Matrix2d halfTransform = 0.5 * getMapFrameToBufferOrderTransformation().cast<double>(); |
| 286 | |
| 287 | // Corners of submap. |
| 288 | Position topLeftPosition = requestedSubmapPosition - halfTransform * requestedSubmapLength.matrix(); |
| 289 | boundPositionToRange(topLeftPosition, mapLength, mapPosition); |
| 290 | if(!getIndexFromPosition(submapTopLeftIndex, topLeftPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) return false; |
| 291 | Index topLeftIndex; |
| 292 | topLeftIndex = getIndexFromBufferIndex(submapTopLeftIndex, bufferSize, bufferStartIndex); |
| 293 | |
| 294 | Position bottomRightPosition = requestedSubmapPosition + halfTransform * requestedSubmapLength.matrix(); |
| 295 | boundPositionToRange(bottomRightPosition, mapLength, mapPosition); |
| 296 | Index bottomRightIndex; |
| 297 | if(!getIndexFromPosition(bottomRightIndex, bottomRightPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) return false; |
| 298 | bottomRightIndex = getIndexFromBufferIndex(bottomRightIndex, bufferSize, bufferStartIndex); |
| 299 | |
| 300 | // Get the position of the top left corner of the generated submap. |
| 301 | Position topLeftCorner; |
| 302 | if(!getPositionFromIndex(topLeftCorner, submapTopLeftIndex, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) return false; |
| 303 | topLeftCorner -= halfTransform * Position::Constant(resolution); |
| 304 | |
| 305 | // Size of submap. |
| 306 | submapBufferSize = bottomRightIndex - topLeftIndex + Index::Ones(); |
| 307 | |
| 308 | // Length of the submap. |
| 309 | submapLength = submapBufferSize.cast<double>() * resolution; |
| 310 | |
| 311 | // Position of submap. |
| 312 | Vector vectorToSubmapOrigin; |
| 313 | getVectorToOrigin(vectorToSubmapOrigin, submapLength); |
| 314 | submapPosition = topLeftCorner - vectorToSubmapOrigin; |
| 315 | |
| 316 | // Get the index of the cell which corresponds the requested |
| 317 | // position of the submap. |
| 318 | return getIndexFromPosition(requestedIndexInSubmap, requestedSubmapPosition, submapLength, submapPosition, resolution, submapBufferSize); |
| 319 | } |
| 320 | |
| 321 | Size getSubmapSizeFromCornerIndeces(const Index& topLeftIndex, const Index& bottomRightIndex, |
| 322 | const Size& bufferSize, const Index& bufferStartIndex) |