Octomap (октри) координаты в пространстве xyx - PullRequest
Новая
       85

Octomap (октри) координаты в пространстве xyx

0 голосов
/ 25 июня 2018

Учитывая octomap::OcTree, как я могу получить декартовы координаты занятых ячеек? 

double printOccupied(boost::shared_ptr<octomap::OcTree> octree) {

    // Get some octomap config data
    auto res = octree->getResolution();
    unsigned int max_depth = octree->getTreeDepth();

    // Iterate over nodes
    int count = 0;
    std::cout << "printOccupied: octree res = " << res << std::endl;
    std::cout << "printOccupied: octree max depth = " << max_depth << std::endl;
    std::cout << "printOccupied: iterating over nodes..." << std::endl;
    for (octomap::OcTree::iterator it = octree->begin(); it != octree->end(); ++it) {
        if (octree->isNodeOccupied(*it) && it.getDepth() < max_depth) {
            count++;
            // Fetching the coordinates in octomap-space
            std::cout << "  x = " << it.getX() << std::endl;
            std::cout << "  y = " << it.getY() << std::endl;
            std::cout << "  z = " << it.getZ() << std::endl;
            std::cout << "  size = " << it.getSize() << std::endl;
            std::cout << "  depth = " << it.getDepth() << std::endl;
            // Then convert to meters???
            auto cell = std::make_tuple(it.getX() * res,
                                        it.getY() * res,
                                        it.getZ() * res);
        }
    }
    std::cout << "printOccupied: number of occupied cells = " << count << std::endl;
}

Когда я передаю octree, который генерируется изпусто PlanningScene Я получаю 0 занятых ячеек, как и ожидалось.Когда я использую сцену, которая, как известно, имеет одну сферу с радиусом 0,05 метра в координатах XYZ (0,1, 0,8, 0,1), в соответствии с системой отсчета сцены (также в метрах), я получаю следующий вывод: 

printOccupied: octree res = 0.02
printOccupied: octree max depth = 16
printOccupied: iterating over nodes...
  x = -327.68
  y = -327.68
  z = -327.68
  size = 655.36
  depth = 1
  x = 327.68
  y = -327.68
  z = -327.68
  size = 655.36
  depth = 1
  x = -491.52
  y = 491.52
  z = -491.52
  size = 327.68
  depth = 2
  x = 327.68
  y = 327.68
  z = -327.68
  size = 655.36
  depth = 1
  x = -92.16
  y = 624.64
  z = 51.2
  size = 20.48
  depth = 6
  x = -81.92
  y = 409.6
  z = 245.76
  size = 163.84
  depth = 3
  x = -419.84
  y = 624.64
  z = 378.88
  size = 20.48
  depth = 6
  x = -409.6
  y = 409.6
  z = 573.44
  size = 163.84
  depth = 3
  x = 327.68
  y = 327.68
  z = 327.68
  size = 655.36
  depth = 1
printOccupied: number of occupied cells = 9

Конечно, необходимо некоторое преобразование, так как эти значения xyz octomap не соответствуют ни одной маленькой сфере, как ожидалось.Что это за конверсия?

1 Ответ

0 голосов
/ 03 июля 2018

Я вижу, что проблема в том, как вы используете итераторы.Октро имеет структуру дерева, а используемый вами итератор перемещается по дереву без учета глубины ячеек.

Глубина отсчитывается от корня дерева, поэтому ячейки, которые вы используетев качестве выходных данных отображаются ячейки высокого уровня, которые, как правило, не следует использовать для проверки столкновения из-за их размера (глубина = 1 - это корень дерева, которое содержит 4 ячейки глубиной 2 ... и это продолжаетсярекурсивно до max_depth, что обычно составляет 16).

Я понимаю, что вы хотите знать, какие листовые ячейки (более мелкие) заняты, и у вас есть один итератор, который может помочь вам сделать это.Вот как я это делаю: 

for(OcTree::leaf_iterator it = octree->begin_leafs(), end = octree->end_leafs(); it != end; ++it){
        // Fetching the coordinates in octomap-space
        std::cout << "  x = " << it.getX() << std::endl;
        std::cout << "  y = " << it.getY() << std::endl;
        std::cout << "  z = " << it.getZ() << std::endl;
        std::cout << "  size = " << it.getSize() << std::endl;
        std::cout << "  depth = " << it.getDepth() << std::endl;
    }
}

Преобразование не требуется, xyz уже находятся в глобальных координатах карты.

Примечание: если вам нужно перемещаться только по ячейкам, которые находятся внутриограничивающий прямоугольник, взгляните на методы octree->begin_leafs_bbx() и end_leafs_bbx(), чтобы создать свой итератор.Если вам нужно ограничить глубину листьев, я думаю, вы можете сделать это и с помощью этих методов.

Надеюсь, это поможет.С уважением,

Adrián

РЕДАКТИРОВАТЬ: изменил код в ответе из-за ошибки в типе возврата begin_leafs().Также отмечено, что begin_leafs() и end_leafs() ведут себя так же, как begin() и end() в соответствии с API Octomap.

...