RasterRangeScan.h

Go to the documentation of this file.

/*
 * Copyright (C) by
 *   MetraLabs GmbH (MLAB), GERMANY
 *  and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Redistribution and modification of this code is strictly prohibited.
 *
 * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
 * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
 * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
 * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#ifndef _MIRA_RASTERRANGESCAN_H_
#define _MIRA_RASTERRANGESCAN_H_

#include <math/Math.h>

#include <geometry/RasterTriangle.h>
#include <transform/Pose.h>
#include <robot/RangeScanInfo.h>
#include <geometry/DistanceLUT.h>

namespace mira { namespace maps {



struct RangeScanCone
{
    Pose2 origin;         // the offset and orientation of the sensor cone origin

    float distanceOffset; // the distance offset from the sensor cone origin to
                          // the sensor 'plane' (start of measurement)

    float coneAngle;      // the cones opening angle
};
template<typename Visitor, bool VisitorUsesWorldDistance=true>
void rasterSensorCone(const RangeScanCone& c,
                      float maxRange, float cellSize, Visitor& visitor);

// Implementation:

template<typename Visitor, bool VisitorUsesWorldDistance>
class RasterSensorConeVisitor
{
public:
    RasterSensorConeVisitor(Visitor& visitor, const DistanceLUT::Region& region, float cellSize, float distOffset) :
        mVisitor(visitor), mRegion(region), mCellSize(cellSize), mDistanceOffset(distOffset) {}

    void operator()(int xl, int xr, int y)
    {
        mVisitor.beginScanline(y);

        DistanceLUT::iterator i = mRegion.getIterator(xl,y);
        for(int x=xl; x<=xr; ++x, ++i)
        {
            if constexpr (VisitorUsesWorldDistance) {
                float min_d = i->minDist * mCellSize - mDistanceOffset;
                float max_d = i->maxDist * mCellSize - mDistanceOffset;
                mVisitor(x,y, min_d, max_d);
            } else {
                mVisitor(x,y, i->minDist, i->maxDist);
            }
        }
    }

private:


    Visitor& mVisitor;
    const DistanceLUT::Region& mRegion;
    float mCellSize;
    float mDistanceOffset;
};


template<typename Visitor, bool VisitorUsesWorldDistance>
inline void rasterSensorCone(const RangeScanCone& c,
                             float maxRange, float cellSize, Visitor& visitor)
{
    float alpha = c.coneAngle / 2.0f; // the half cone angle

    float x = c.origin.x();
    float y = c.origin.y();
    float phi = c.origin.phi();

    float phi_left  = phi - alpha;
    float phi_right = phi + alpha;

    maxRange += c.distanceOffset;
    maxRange += cellSize;

    if (cosf(alpha) < std::numeric_limits<float>::min()) {
        MIRA_THROW(XInvalidConfig, "rasterSensorCone: "
                                   "Invalid cone angle " << c.coneAngle << " (must be 0 < angle < 180deg)");
        // avoid div-by-zero or negative maxRange!
    }
    maxRange /= cosf(alpha);

    // ending position of left ray of the cone
    float x_left = cosf(phi_left) * maxRange + x;
    float y_left = sinf(phi_left) * maxRange + y;

    // ending position of right ray of the cone
    float x_right = cosf(phi_right) * maxRange + x;
    float y_right = sinf(phi_right) * maxRange + y;

    int xi, yi;
    float xi_eps, yi_eps;

    // cell of the origin
    xi = (int)floor(x/cellSize);
    yi = (int)floor(y/cellSize);

    // get the position of the origin within the cell (for sub-cell precision)
    xi_eps = x/cellSize - (float)xi;
    yi_eps = y/cellSize - (float)yi;

    //std::cout << "x:" << x << ",y:"<<y<<",xi:"<<xi<<",yi"<<yi<<",xi_eps"<<xi_eps<<",yi_eps"<<yi_eps<< std::endl;

    // compute the index of the cells where the left and right ray end
    int xi_left, yi_left;
    int xi_right, yi_right;

    xi_left = (int)floor(x_left/cellSize);
    yi_left = (int)floor(y_left/cellSize);

    xi_right = (int)floor(x_right/cellSize);
    yi_right = (int)floor(y_right/cellSize);

    // get region we need from the distance LUT
    int ri = maxRange / cellSize;
    DistanceLUT::Region r = DistanceLUT::instance().getRegion(Point2f(xi_eps, yi_eps),
                                                              Rect2i(Point2i(-ri,-ri),
                                                                     Point2i(ri,ri)));

    // and raster the triangle, via our RasterSensorConeVisitor, that translates
    // the calls to the user specified visitor.
    visitor.beginScan(c);
    typedef RasterSensorConeVisitor<Visitor, VisitorUsesWorldDistance> ConeVisitor;
    ConeVisitor coneVisitor(visitor, r, cellSize, c.distanceOffset);
    Point2i center(0,0);
    Point2i left(xi_left-xi,yi_left-yi);
    Point2i right(xi_right-xi,yi_right-yi);
    rasterTriangle(center, left, right, coneVisitor);
}


}}

#endif