| 407 | |
| 408 | |
| 409 | bool RdbPointcloud::getBoundingBox( |
| 410 | Eigen::Vector4d& minimum, |
| 411 | Eigen::Vector4d& maximum |
| 412 | ) |
| 413 | { |
| 414 | if (m_pointcloud.pointAttribute().exists("riegl.xyz")) |
| 415 | { |
| 416 | using namespace riegl::rdb::pointcloud; |
| 417 | QueryStat stat = m_pointcloud.stat(); |
| 418 | GraphNode root = stat.index(); |
| 419 | |
| 420 | std::array<std::array<double, 3>, 2> limits; // 0=min, 1=max |
| 421 | stat.minimum(root.id, "riegl.xyz", DOUBLE, limits[0].data()); |
| 422 | stat.maximum(root.id, "riegl.xyz", DOUBLE, limits[1].data()); |
| 423 | |
| 424 | const double inf = std::numeric_limits<double>::infinity(); |
| 425 | minimum = Eigen::Vector4d(+inf, +inf, +inf, 1.0); |
| 426 | maximum = Eigen::Vector4d(-inf, -inf, -inf, 1.0); |
| 427 | const Eigen::Matrix4d pose = crsPose(); |
| 428 | |
| 429 | for (int i = 0; i < 8; ++i) // transform box corners to crs... |
| 430 | { |
| 431 | const Eigen::Vector4d corner = pose * Eigen::Vector4d( |
| 432 | limits[(i >> 0) & 1][0], |
| 433 | limits[(i >> 1) & 1][1], |
| 434 | limits[(i >> 2) & 1][2], |
| 435 | 1.0 |
| 436 | ); |
| 437 | minimum = minimum.cwiseMin(corner); // ...and update |
| 438 | maximum = maximum.cwiseMax(corner); // bounding box! |
| 439 | } |
| 440 | return true; |
| 441 | } |
| 442 | return false; |
| 443 | } |
| 444 | |
| 445 | |
| 446 | //---< RdbPointcloud::PRIVATE >------------------------------------------------- |