castRay always returns empty
Hi
I'm doing the typical casting a ray to see if it is occupied.
I wasn't understanding the results so I decided to do a reality check, to make sure I was using the function correctly.
What I did was to transverse the tree and use the coordinates of the center of the occupied cells to call the castRay function.
octomap::point3d min; min.x() = -range; min.y() = -range; min.z() = -1;
octomap::point3d max; max.x() = range; max.y() = range; max.z() = 4;
std::cout << "printing occupancy \n";
for(octomap::OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(min,max), endLoop=octree->end_leafs_bbx();
it!= endLoop;
++it)
{
octomap::OcTreeNode * temp = octree->search(it.getKey());
if (temp != NULL && octree->isNodeOccupied(temp))
{
std::cout << "(" << it.getX() << "; " << it.getY()<< "; " << it.getZ() << ") ";
std::cout << it.getCoordinate() << " ";
// Testing ray
octomath::Vector3 end;
bool occupied = octree->castRay(octomath::Vector3(0.f, 0.f, 0.f),
it.getCoordinate(),
end,
false,
range+10
);
std::cout << " according to ray it is occupied? "<< occupied << "\n";
}
}
std::cout << "\n";
I was expecting to always get
(2.55; 0.45; 0.85) (2.55 0.45 0.85) according to ray it is occupied? 1
But instead I got
(2.55; 0.45; 0.85) (2.55 0.45 0.85) according to ray it is occupied? 0
In a lot of different positions.
Why is it considered occupied as a node but on the ray cast it is empty?