ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

octomap castRay returns a point in opposite direction

asked 2011-08-29 13:43:42 -0500

updated 2011-08-31 05:35:33 -0500

PointOutT origin; 

origin.x = -1;
origin.y = 1.2;
origin.z = -0.5;

PointOutT d; // direction

d.x = 0;
d.y = 1;
d.z = 0;

tree.castRay(origin,d,end,true,-1)

returns true and end=(-1.0198,0.9802,-0.5198)

Can I be assured that this happens only if there is no point in the actual direction .

More Details:

PointOutT is of type PointXYZRGBCamSL as defined in: https://github.com/aa755/scene_labelling_rgbd/blob/release2/scene_processing/src/includes/point_types.h

tree is a member of my class OccupancyMap ... I have included the relevant snippets . How do I get the version of OctomapStack?

class OccupancyMap
{
float resolution;
octomap::OcTreeROS tree;
pcl::PointCloud<pcl::PointXYZ> xyzcloud;

public:
OccupancyMap(pcl::PointCloud<PointOutT> & cloud, float resolution_ = 0.02) : tree(resolution_)
{
    cloudSeg=& cloud;
    resolution = resolution_;
    convertToXYZ(cloud, xyzcloud);
    tree.insertScan(xyzcloud, convertFromVector(cloud.sensor_origin_), -1, true);
}

static void convertToXYZ(const pcl::PointCloud<PointOutT> &cloud, pcl::PointCloud<pcl::PointXYZ> & cloudxyz)
{
    cloudxyz.points.resize(cloud.size());
    for (size_t i = 0; i < cloud.size(); i++)
    {
        cloudxyz.points[i].x = cloud.points[i].x;
        cloudxyz.points[i].y = cloud.points[i].y;
        cloudxyz.points[i].z = cloud.points[i].z;
    }
}

...

version is ros-diamondback-octomap-mapping_0.3.3-s1313567137~maverick_amd64.deb .Please let me know if you have more questions .. I can send the entire code and pointcloud if you wish

edit retag flag offensive close merge delete

Comments

See my updated answer below. Looks like a bug, but I strongly recommend using either maxRange or set ignoreUnknown to false.
AHornung gravatar image AHornung  ( 2011-08-31 04:59:05 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2011-08-29 20:38:04 -0500

AHornung gravatar image

updated 2011-09-06 03:44:37 -0500

That should not happen. It should return "true" only if an occupied cell was hit, and that in the right direction. Can you post more code that produces this behavior, what type is PointOutT and "tree"? Which version of octomap / the octomap_mapping stack are you using? What does your map look like, from your last sentence I assume that you are raycasting into space where there are no occupied voxels?


After some more testing (you are indeed using the latest version), it looks like an overflow / wrap around effect. You don't specify a maximum range to raycast and at some point hit the boundary of the octree. Instead of returning with an error, the ray continues on the other side. This will be fixed in the next release. Until then, just specify a sensible maxRange for the raycast. This will also speed up the computation because you won't run until the end of the coordinate system if there is nothing to hit! Another option would be to set ignoreUnknown to false.


Update: OctoMap was patched to fix this undesired behavior (version 1.2.1). The octomap package in the trunk at alufr-ros-pkg was already updated, a fixed 0.3.4 release will be available soon.

edit flag offensive delete link more

Comments

To determine your version of octomap: how do you use it / install it? From your answer I assume you use the "octomap" package and the "octomap_ros" wrapper? You can just run "roscd octomap_mapping" and then have a look at the CMakeLists, there is a version number there.
AHornung gravatar image AHornung  ( 2011-08-30 06:33:52 -0500 )edit
Also: what is your ROS distribution, diamondback?
AHornung gravatar image AHornung  ( 2011-08-30 06:34:52 -0500 )edit
Thanks. I indeed observed that sometimes castRay was taking minutes! So I implemented my own castRay in a dirty way(move into the desired direction in steps of resolution and keep checking occupancy) . Does the octomap's castRay do it in a smarter way(maybe by exploiting the octree structure?)
aa755 gravatar image aa755  ( 2011-08-31 05:47:46 -0500 )edit

Question Tools

Stats

Asked: 2011-08-29 13:43:42 -0500

Seen: 1,417 times

Last updated: Sep 06 '11