が与えられた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 になります。シーンの参照フレーム (メートル) に従って、xyz 座標 (0.1、0.8、0.1) で半径 0.05 メートルの単一の球体を持つことがわかっているシーンを使用すると、次の出力が得られます。
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
これらの octomap xyz 値は期待どおりに単一の小さな球体に対応していないため、確かに変換が必要です。この変換は何ですか？