[feature/slam2d] Add the option to mark no-hits as free space

[jcmonteiro]

Why

Avoid the cone effect in places such as corridors when the scan does not hit anything.

Summary

Instead of completely ignoring a no-hit while mapping, the algorithm can now mark the cells as free-space.

Example

The images below illustrate the difference in behavior.

Before

After

[eupedrosa]

I like the idea, but I have some concerns about the implementation.

We already have something similar, it is the *truncated rays". https://github.com/iris-ua/iris_lama/blob/2e665154fc08b55b56302c89cfb76fef034d2a65/src/slam2d.cpp#L260-L268

We could implement something similar, and remove the need to pass more function variables and create a new surface point cloud. Like with the truncated_ray_ just tell the mapper that the marking has a limit. But instead of manipulating the start point we manipulate the hit point.

This may confuse the user as to what is a maximum_range and what is a truncated_maximum_range. But in my opinion I prefer a cleaner implementation.

As it is, the maximum_range filters the ranges that are accepted for mapping.

Thus this makes sense to you?

[jcmonteiro]

I agree. I also dislike increasing the number of arguments. So, let me see if I got it right. When one sets truncated_ray_ to something greater than zero all hits are still counted by the algorithm, but there might be a region of length AB - truncated_ray_ from the start of the scan that will not be marked as free space. Is that correct?

I like the idea. Do you agree on creating an extra member variable, say no_hit_threshold (please suggest a better name...), such that if a scan has length <= max_range_ + no_hit_threshold_ and > max_range_ the algorithm does not mark the hit. The feature could fit together with truncated_ray_. The code could look something like this:

const double upper_bound_to_ignore_hit = max_range_ + no_hit_threshold_;

for (size_t i = 0; i < num_points; ++i){
    Vector3d start = wso;
    Vector3d hit = tf * surface->points[i];
    Vector3d AB = hit - wso;
    const double scan_length = AB.norm();

    if (truncated_ray_ > 0.0){
        double truncate_size = std::min(scan_length, truncated_ray_);
        start = hit - AB.normalized() * truncate_size;
    }

    Vector3ui mhit = occupancy_map_->w2m(hit);

    if (scan_length <= max_range_)
    {
        bool changed = occupancy_map_->setOccupied(mhit);
        if ( changed ) distance_map_->addObstacle(mhit);
        occupancy_map_->computeRay(occupancy_map_->w2m(start), mhit, free);
    }
    else if (scan_length <= upper_bound_to_ignore_hit)
        occupancy_map_->computeRay(occupancy_map_->w2m(start), mhit, free);
}

[jcmonteiro]

We could also get around creating the new variable no_hit_threshold_ altogether, and just assume that everything above max_range_ should not be considered a hit. Removing the first line from the code snippet above and changing the else if to an else.

[jcmonteiro]

Maybe change start = hit - AB.normalized() * truncate_size; to start = hit - AB * truncate_size / scan_length as well to avoid calculating the norm more than once.

[eupedrosa]

My suggestion is something like this:

for (size_t i = 0; i < num_points; ++i){
        Vector3d start = wso;
        Vector3d hit = tf * surface->points[i];
        bool mark_hit = true;

        if (truncated_range_ > 0.0 {
            Vector3d AB = wso - hit;
            double truncate_size = std::min(AB.norm(), truncated_range_);
            hit  = start + AB.normalized() * truncate_size;

            mark_hit = (truncate_size < truncated_range_);

        else if (truncated_ray_ > 0.0){
            // truncated_range_ takes priority ..
            Vector3d AB = hit - wso;
            double truncate_size = std::min(AB.norm(), truncated_ray_);
            start = hit - AB.normalized() * truncate_size;
        }

        Vector3ui mhit = occupancy_map_->w2m(hit);
        if (mark_hit == true){  
            bool changed = occupancy_map_->setOccupied(mhit);
            if ( changed ) distance_map_->addObstacle(mhit);
        } else {
            free.push_back(mhit);
        }

        occupancy_map_->computeRay(occupancy_map_->w2m(start), mhit, free);
    }
}

The truncatedrange and truncatedray should be disjoint.

The extra steps is to add the variable to the SLAM object and expose it in the SLAM::Options. What is your opinion?

We could also get around creating the new variable no_hitthreshold altogether, and just assume that everything above maxrange should not be considered a hit. Removing the first line from the code snippet above and changing the else if to an else.

There is no maxrange in the SLAM object. We need to add a new anyway. But we can goes this route. It implies changing truncated_range_ to max_range_ (in my suggested code) and then go to the ROS node and remove the code that filters the ranges.

However, remember that doing this can add addicional computation effor to the mapping, "half-ranges" will now be integrated.

[jcmonteiro]

I have no preference for one solution over the other to tell you the truth... But I didn't get why should truncated_range_ and truncated_ray_ be disjoint. Can't both features coexist?

But we can goes this route. It implies changing truncatedrange to maxrange (in my suggested code) and then go to the ROS node and remove the code that filters the ranges.

I think the code to filter the ranges can stay there, but it would be better to have an accessor method, e.g. lama::Slam2D::getMaxRange, to return the actual maximum range. Then, by default, if the user does not set the truncated_range_ it defaults to max_range_ and the expected behavior remains the same.

However, remember that doing this can add addicional computation effor to the mapping, "half-ranges" will now be integrated.

It should not make any difference for existing users. But I noticed an increase in computational effort and memory usage, nothing that really hinders performance though.

Besides, why would you add that line free.push_back(mhit)?

[eupedrosa]

But I didn't get why should truncatedrange and truncatedray be disjoint. Can't both features coexist?

The truncatedrange goes from its origin to the "middle" of the ray, and the truncated_ray goes from the "middle" to the end of the ray. If you merge them, there may be a "middle" to "middle" ray.

I usually seperate a problem into disjoint sub-problems and only then I try to join then. That is way, for now, I ignored the "middle" to "middle".

I think the code to filter the ranges can stay there, but it would be better to have an accessor method, e.g. lama::Slam2D::getMaxRange, to return the actual maximum range. Then, by default, if the user does not set the truncatedrange it defaults to maxrange and the expected behavior remains the same.

If the the logic of if (truncated_range_ > 0) exists, you avoid unecessary code execution.

I think my previoud code can be improved.

       if (truncated_range_ > 0.0) {
            Vector3d AB = wso - hit;
            double truncate_size = std::min(AB.norm(), truncated_range_);
            hit  = start + AB.normalized() * truncate_size;

            mark_hit = (truncate_size < truncated_range_);
        }

        if ((truncated_ray_ > 0.0) and mark_kit ){
            // truncated_range_ takes priority ..
            Vector3d AB = hit - wso;
            double truncate_size = std::min(AB.norm(), truncated_ray_);
            start = hit - AB.normalized() * truncate_size;
        }

We can truncate the ray, if we do not truncate the range.

[jcmonteiro]

If the the logic of if (truncatedrange > 0) exists, you avoid unecessary code execution.

I agree with this. The thing is that if we remove the filtering logic from the ROS side it will increase the cost of creating the surface (which might be negligible). I'll make the changes as you suggested (most likely tomorrow or Saturday) and benchmark to see if filtering in ROS makes a difference or not. It most likely won't and then I'll remove as you suggested.

I think my previous code can be improved.

I'll probably avoid calculating the norm multiple times as well.

[jcmonteiro]

I've made the changes according to our discussion. There's still the basic filtering in ROS, which I didn't change. I've updated iris-ua/iris_lama_ros#28 and there are only two new lines of code :grinning:.

[eupedrosa]

Great. I will play with this PR and #26 during the weekend, then I will merge them. I guess I can take the opportunity to release a new version.