RasterTransformation.h

Go to the documentation of this file.

/*
 * Copyright (C) 2013 by
 *   MetraLabs GmbH (MLAB), GERMANY
 * and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Contact: info@mira-project.org
 *
 * Commercial Usage:
 *   Licensees holding valid commercial licenses may use this file in
 *   accordance with the commercial license agreement provided with the
 *   software or, alternatively, in accordance with the terms contained in
 *   a written agreement between you and MLAB or NICR.
 *
 * GNU General Public License Usage:
 *   Alternatively, this file may be used under the terms of the GNU
 *   General Public License version 3.0 as published by the Free Software
 *   Foundation and appearing in the file LICENSE.GPL3 included in the
 *   packaging of this file. Please review the following information to
 *   ensure the GNU General Public License version 3.0 requirements will be
 *   met: http://www.gnu.org/copyleft/gpl.html.
 *   Alternatively you may (at your option) use any later version of the GNU
 *   General Public License if such license has been publicly approved by
 *   MLAB and NICR (or its successors, if any).
 *
 * 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_RASTERTRANSFORMATION_H_
#define _MIRA_RASTERTRANSFORMATION_H_

#include <math/Truncate.h>
#include <geometry/Rect.h>
#include <geometry/Bresenham.h>
#include <transform/Pose.h>

namespace mira {


class RasterTransformation
{

public:
    RasterTransformation(const Rect2i & srcArea = Rect2i(0,0,1,1),
                         const Rect2i & tgtArea = Rect2i(0,0,1,1),
                         const Point2d & srcRef = Point2d(0.0, 0.0),
                         const Point2d & tgtRef = Point2d(0.0, 0.0),
                         double scale = 1.0,
                         double rotate = 0.0,
                         bool dense = false);

    RasterTransformation(const RasterTransformation& other);

    RasterTransformation& operator=(const RasterTransformation& other);

public:
    class iterator
    {
        //  The basic idea is to go through the target raster line by line and
        //  determine the corresponding line in the source raster. Line sampling
        //  distance is the smaller of the 2 raster cell sizes, so we do not miss
        //  cells in either source or target.
        //  For each line the exact real-valued start and end points in source
        //  and target raster are calculated (with respect to the transformation
        //  config). Points on the line are then defined by 3 changing plus 1
        //  constant coordinates (target x, source x, source y = changing,
        //  target y = fixed. Note that source y may be fixed, but only for
        //  particular rotation.)
        //  Line start/end points are clipped to the area borders so we do not
        //  access outside the defined area.
        //  Fast iteration over the line is done using a Bresenham algorithm.

        friend class RasterTransformation;

    public:
        iterator();

        iterator(const iterator& other);

        iterator(RasterTransformation* rt);

    public:

        iterator& operator=(const iterator& other);

        bool operator==(const iterator& other) const;

        bool operator!=(const iterator& other) const;

        const iterator& operator++();

        bool isValid() const { return mLineValid; }

        inline int srcX() const { return mBresenham.axis(0).current; }

        inline int srcY() const { return mBresenham.axis(1).current; }

        inline int tgtX() const { return mBresenham.axis(2).current; }

        inline int tgtY() const { return mTY; }

    protected:

        bool initLine();
        void setToBegin();

    public:

        RasterTransformation* mT;

        GeneralBresenhamLineIterator<4, double, 3, 10000> mBresenham;

        double mCurrentLine;
        int mTY;
        bool mLineValid;
    };

public:

    bool operator==(const RasterTransformation& other) const;

public:

    const iterator& begin() const { return mBegin; }

protected:

    struct Configuration {
        Rect2i srcArea, tgtArea;
        Point2d srcRef, tgtRef;

        double scale;
        double rotate;

        bool dense;

        bool operator==(const Configuration& other) const;
        bool targetAreaIsEmpty() const;
    };

    struct Impl {
        double sinRot, cosRot;  // sin/cos of rotation angle
        double dxStart, dxEnd;  // target x of line start/end point
        double lineStep;        // distance between lines in target (tgt y)
        Point3d rasterStep;     // vector between 2 subsequent sample raster
                                // points on one 3d line (src x, src y, tgt x)

        Impl(const Configuration& config);
    };

    Configuration mConfig;
    Impl mImpl;
    iterator mBegin;


protected:

    static bool clipLine(const Rect2i & area, Point3d & p1, Point3d & p2);
};

// implementation section: RasterTransformation class implementation

inline bool RasterTransformation::Configuration::
operator==(const RasterTransformation::Configuration& other) const
{
    return ((srcArea == other.srcArea) &&
            (tgtArea == other.tgtArea) &&
            (srcRef == other.srcRef) &&
            (tgtRef == other.tgtRef) &&
            (scale == other.scale) &&
            (rotate == other.rotate) &&
            (dense == other.dense));
}

inline bool RasterTransformation::Configuration::targetAreaIsEmpty() const {
    return ((tgtArea.width() == 0) ||
            (tgtArea.height() == 0) ||
            (scale == 0));
}

inline RasterTransformation::Impl::Impl(const RasterTransformation::Configuration& config)
{
    if ((config.srcArea.width() < 1) || (config.srcArea.height() < 1) ||
        (config.tgtArea.width() < 1) || (config.tgtArea.height() < 1))
        return;

    lineStep = std::min(1., std::abs(config.scale));
    if (config.dense)
        lineStep /= sqrt(2.);

    // the truncate adds tiny inaccuracy in the general case, but ensures
    // we meet expectations in special cases where sin/cos should be 0/1
    // (which otherwise we don't, due to numerical precision limits)
    sinRot = mira::truncate(std::sin(config.rotate), 15);
    cosRot = mira::truncate(std::cos(config.rotate), 15);

    dxStart  =  config.tgtArea.x0() - config.tgtRef.x();
    dxEnd    =  config.tgtArea.x1() - config.tgtRef.x();

    rasterStep = Point3d(cosRot * (dxEnd - dxStart) / config.scale,
                         sinRot * (dxEnd - dxStart) / config.scale,
                         dxEnd - dxStart);

    double length = std::max(std::hypot(rasterStep.x(), rasterStep.y()),
                             std::abs(rasterStep.z()));
    rasterStep /= length;
}

inline RasterTransformation::RasterTransformation(const Rect2i & srcArea,
                                                  const Rect2i & tgtArea,
                                                  const Point2d & srcRef,
                                                  const Point2d & tgtRef,
                                                  double scale,
                                                  double rotate,
                                                  bool dense)
    : mConfig{Rect2i(srcArea.x0(), srcArea.y0(), srcArea.width()-1, srcArea.height()-1),
              Rect2i(tgtArea.x0(), tgtArea.y0(), tgtArea.width()-1, tgtArea.height()-1),
              srcRef, tgtRef, scale, rotate, dense},
     mImpl(mConfig),
     mBegin(this)
{
}

inline RasterTransformation::RasterTransformation(const RasterTransformation& other)
    : mConfig(other.mConfig),
      mImpl(other.mImpl),
      mBegin(this)
{
}

inline RasterTransformation& RasterTransformation::operator=(const RasterTransformation& other)
{
    mConfig = other.mConfig;
    mImpl = other.mImpl;
    mBegin = iterator(this);

    return *this;
}

inline bool RasterTransformation::operator==(const RasterTransformation& other) const
{
    return mConfig == other.mConfig;
}


inline RasterTransformation::iterator::iterator()
    : mT(NULL), mLineValid(false)
{
}

inline RasterTransformation::iterator::iterator(RasterTransformation* rt)
    : mT(rt)
{
    if (!rt->mConfig.targetAreaIsEmpty())
        setToBegin();
}

inline RasterTransformation::iterator::iterator(const RasterTransformation::iterator& other)
    : mT(other.mT),
      mBresenham(other.mBresenham),
      mCurrentLine(other.mCurrentLine),
      mTY(other.mTY),
      mLineValid(other.mLineValid)
{
}

inline RasterTransformation::iterator&
RasterTransformation::iterator::operator=(const RasterTransformation::iterator& other)
{
    mT = other.mT;
    mBresenham = other.mBresenham;
    mCurrentLine = other.mCurrentLine;
    mTY = other.mTY;
    mLineValid = other.mLineValid;

    return *this;
}

inline bool RasterTransformation::clipLine(const Rect2i & area,
                                           Point3d & p1, Point3d & p2)
{
    // source: openCV
    // adaptation: 2d   -> 3d (precisely: 3d line clipped to 2d area),
    //             int  -> double
    //             size -> area (i.e. left/top != 0)

    int left   = area.x0();
    int right  = area.x1();
    int top    = area.y0();
    int bottom = area.y1();

    double & x1 = p1.x();
    double & y1 = p1.y();
    double & z1 = p1.z();
    double & x2 = p2.x();
    double & y2 = p2.y();
    double & z2 = p2.z();

    int c1 = (x1 < left) + ((x1 > right) << 1) + ((y1 < top) << 2) + ((y1 > bottom) << 3);
    int c2 = (x2 < left) + ((x2 > right) << 1) + ((y2 < top) << 2) + ((y2 > bottom) << 3);

    if( ((c1 & c2) == 0) && ((c1 | c2) != 0 )) // one or both points outside
                                               // but not both to the same side!
    {
        double a;
        if( c1 & 12 ) // y1 is outside
        {
            // move p1 to area top/bottom
            a = c1 < 8 ? top : bottom;
            x1 += (a - y1) * (x2 - x1) / (y2 - y1);
            z1 += (a - y1) * (z2 - z1) / (y2 - y1);
            y1 = a;
            c1 = (x1 < left) + (x1 > right) * 2;
        }
        if( c2 & 12 ) // y2 is outside
        {
            // move p2 to area top/bottom
            a = c2 < 8 ? top : bottom;
            x2 += (a - y2) * (x2 - x1) / (y2 - y1);
            z2 += (a - y2) * (z2 - z1) / (y2 - y1);
            y2 = a;
            c2 = (x2 < left) + (x2 > right) * 2;
        }
        if( ((c1 & c2) == 0) && ((c1 | c2) != 0) ) // one or both x outside
                                                   // but not both to the same side!
        {
            if( c1 ) // x1 is outside
            {
                // move p1 to area left/right
                a = c1 == 1 ? left : right;
                y1 += (a - x1) * (y2 - y1) / (x2 - x1);
                z1 += (a - x1) * (z2 - z1) / (x2 - x1);
                x1 = a;
                c1 = 0;
            }
            if( c2 ) // x2 is outside
            {
                // move p2 to area left/right
                a = c2 == 1 ? left : right;
                y2 += (a - x2) * (y2 - y1) / (x2 - x1);
                z2 += (a - x2) * (z2 - z1) / (x2 - x1);
                x2 = a;
                c2 = 0;
            }
        }
    }

    return (c1 | c2) == 0;
}

inline bool RasterTransformation::iterator::initLine()
{
    mTY = floor(mCurrentLine + 0.5 * mT->mImpl.lineStep);

    if (mTY > mT->mConfig.tgtArea.y1())
        return (mLineValid = false);

    double dy = mCurrentLine + 0.5f * mT->mImpl.lineStep - mT->mConfig.tgtRef.y();

    Point3d start(mT->mConfig.srcRef.x()
                  + (mT->mImpl.cosRot * mT->mImpl.dxStart - mT->mImpl.sinRot * dy) / mT->mConfig.scale,
                  mT->mConfig.srcRef.y()
                  + (mT->mImpl.sinRot * mT->mImpl.dxStart + mT->mImpl.cosRot * dy) / mT->mConfig.scale,
                  mT->mConfig.tgtArea.x0());

    Point3d end(mT->mConfig.srcRef.x()
                + (mT->mImpl.cosRot *  mT->mImpl.dxEnd  - mT->mImpl.sinRot * dy) / mT->mConfig.scale,
                mT->mConfig.srcRef.y()
                + (mT->mImpl.sinRot *  mT->mImpl.dxEnd  + mT->mImpl.cosRot * dy) / mT->mConfig.scale,
                mT->mConfig.tgtArea.x1());

    Point3d startClipped = start;

    // clip start and end to make sure we don't access outside the designated src area
    mLineValid = clipLine(mT->mConfig.srcArea, startClipped, end);

    // convex area -> there can be no separate valid lines
    // if we hit an empty line after begin, we are already at the end
    if (!mLineValid)
        return false;

    // go to the next raster point inside the clipped interval
    // (advance to multiple of RasterStep)
    if (startClipped != start)
    {
        double steps = 0;

       // normally these values are all the same
       // but because of (extremely rare) numerical issues we rather play safe here, taking the max

        if (mT->mImpl.rasterStep.x() != 0)
            steps = std::max(steps, std::ceil((startClipped.x() - start.x()) / mT->mImpl.rasterStep.x()));

        if (mT->mImpl.rasterStep.y() != 0)
            steps = std::max(steps, std::ceil((startClipped.y() - start.y()) / mT->mImpl.rasterStep.y()));

        if (mT->mImpl.rasterStep.z() != 0)
            steps = std::max(steps, std::ceil((startClipped.z() - start.z()) / mT->mImpl.rasterStep.z()));

        startClipped = start + steps * mT->mImpl.rasterStep;
    }

    // try to avoid sample points near cell borders, as those are sensitive to
    // small deviations (e.g. due to floating point resolution limits).
    // unfortunately it is not possible to optimally solve this issue in general.
    // it seems to help in some cases (and not hurt ever) to move the starting
    // sample point away from the cell border if it is close.
    if (std::abs(startClipped.z() - std::round(startClipped.z())) < 0.25)
    {
        startClipped += mT->mImpl.rasterStep/2;
    }

    // check if we stepped beyond end
    if (((startClipped.x() > start.x()) && (startClipped.x() > end.x())) ||
        ((startClipped.x() < start.x()) && (startClipped.x() < end.x())) )
    {
        return (mLineValid = false);
    }

    // additional 4th axis controls the step size
    Point3d distance = end - startClipped;
    double l = std::max(std::hypot(distance.x(), distance.y()), std::abs(distance.z()));

    Point<double, 4> startBresenham;
    startBresenham[0] = startClipped.x();
    startBresenham[1] = startClipped.y();
    startBresenham[2] = startClipped.z();
    startBresenham[3] = 0;

    Point<double,4> endBresenham;
    endBresenham[0] = end.x();
    endBresenham[1] = end.y();
    endBresenham[2] = end.z();
    if (mT->mConfig.dense)
        endBresenham[3] = sqrt(2.)*l;
    else
        endBresenham[3] = l;

    mBresenham.init(startBresenham, endBresenham);

    return true;
}

inline const RasterTransformation::iterator&
RasterTransformation::iterator::operator++()
{
    if (mBresenham.hasNext())
    {
        ++mBresenham;
        return *this;
    }

    mCurrentLine += mT->mImpl.lineStep;
    initLine();

    return *this;
}

inline void RasterTransformation::iterator::setToBegin()
{
    mCurrentLine = mT->mConfig.tgtArea.y0();
    while (!initLine())
    {
        if (mCurrentLine >= mT->mConfig.tgtArea.y1())
            return;

        mCurrentLine += mT->mImpl.lineStep;
    }
}


} // namespace

#endif