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

Question

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

Учитывая 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

c++moveit octomap octree ros

Решение

Другие решения

Других решений пока нет …

Источник

Accepted Answer

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

Глубина отсчитывается от корня дерева, поэтому ячейки, которые вы выводите в качестве выходных данных, являются ячейками высокого уровня, которые, как правило, не должны использоваться для проверки столкновений из-за их размера (глубина = 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() создать свой итератор. Если вам нужно ограничить глубину листьев, я думаю, что вы можете сделать это и с помощью этих методов.

Надеюсь, это поможет. С наилучшими пожеланиями,

Адриан

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

0