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);
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
    //  parameters). 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);

    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 clipLine(const Rect2i & area, Point3d & p1, Point3d & p2);

        bool initLine();
        void setToBegin();
        void setToEnd();

    public:

        RasterTransformation* mT;

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

        double mCurrentLine;
        int mTY;
        bool mLineValid;
    };

public:

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

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

public:

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

protected:

    Rect2i mSrcArea, mTgtArea;
    Rect2d mSrcAreaF;
    Point2d mSrcRef, mTgtRef;

    double mScale;
    double mRotate;

    bool mDense;

    iterator mBegin;


    double mSinRot, mCosRot;  // sin/cos of rotation angle
    double mDxStart, mDxEnd;  // target x of line start/end point
    double mLineStep;         // distance between lines in target (tgt y)
    Point3d mRasterStep;      // vector between 2 subsequent sample raster
                              // points on one 3d line (src x, src y, tgt x)
};

// implementation section: RasterTransformation class implementation

inline RasterTransformation::
RasterTransformation(const Rect2i & srcArea,
                     const Rect2i & tgtArea,
                     const Point2d & srcRef,
                     const Point2d & tgtRef,
                     double scale,
                     double rotate,
                     bool dense)
                    : mSrcArea(srcArea.x0(), srcArea.y0(), srcArea.width()-1, srcArea.height()-1),
                      mTgtArea(tgtArea.x0(), tgtArea.y0(), tgtArea.width()-1, tgtArea.height()-1),
                      mSrcRef(srcRef),
                      mTgtRef(tgtRef),
                      mScale(scale),
                      mRotate(rotate),
                      mDense(dense)
{
    if ((srcArea.width() < 1) || (srcArea.height() < 1) || (tgtArea.width() < 1) || (tgtArea.height() < 1))
        return;

    mLineStep = std::min(1., std::abs(mScale));
    if (mDense)
        mLineStep /= 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)
    mSinRot = mira::truncate(std::sin(mRotate), 15);
    mCosRot = mira::truncate(std::cos(mRotate), 15);

    mDxStart  =  mTgtArea.x0() - mTgtRef.x();
    mDxEnd    =  mTgtArea.x1() - mTgtRef.x();

    mRasterStep = Point3d(mCosRot * (mDxEnd - mDxStart) / mScale,
                          mSinRot * (mDxEnd - mDxStart) / mScale,
                          mDxEnd - mDxStart);

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

    mBegin.mT = this;

    if ((mTgtArea.width() == 0) || (mTgtArea.height() == 0) || (mScale == 0))
        return;

    mBegin.setToBegin();
}

inline bool RasterTransformation::
operator==(const RasterTransformation& other) const
{
    return mSrcArea == other.mSrcArea &&
           mTgtArea == other.mTgtArea &&
           mSrcRef  == other.mSrcRef  &&
           mTgtRef  == other.mTgtRef  &&
           mScale   == other.mScale   &&
           mRotate  == other.mRotate  &&
           mDense   == other.mDense;
}

inline bool RasterTransformation::
operator!=(const RasterTransformation& other) const
{
    return !(*this == other);
}


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

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::iterator::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(), right  = area.x1(),
        top  = area.y0(), bottom = area.y1();

    int c1, c2;

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

    c1 = (x1 < left) + ((x1 > right) << 1) + ((y1 < top) << 2) + ((y1 > bottom) << 3);
    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->mLineStep);

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

    double dy = mCurrentLine + 0.5f * mT->mLineStep - mT->mTgtRef.y();

    Point3d start(mT->mSrcRef.x()
                  + (mT->mCosRot * mT->mDxStart - mT->mSinRot * dy) / mT->mScale,
                  mT->mSrcRef.y()
                  + (mT->mSinRot * mT->mDxStart + mT->mCosRot * dy) / mT->mScale,
                  mT->mTgtArea.x0());

    Point3d end(mT->mSrcRef.x()
                + (mT->mCosRot *  mT->mDxEnd  - mT->mSinRot * dy) / mT->mScale,
                mT->mSrcRef.y()
                + (mT->mSinRot *  mT->mDxEnd  + mT->mCosRot * dy) / mT->mScale,
                mT->mTgtArea.x1());

    Point3d startClipped = start;

    // clip start and end to make sure we don't access outside the designated src area
    mLineValid = clipLine(mT->mSrcArea, 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->mRasterStep.x() != 0)
            steps = std::max(steps, std::ceil((startClipped.x() - start.x()) / mT->mRasterStep.x()));

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

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

        startClipped = start + steps * mT->mRasterStep;
    }

    // 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->mRasterStep/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->mDense)
        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->mLineStep;
    initLine();

    return *this;
}

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

        mCurrentLine += mT->mLineStep;
    }
}


} // namespace

#endif