Curve.h

Go to the documentation of this file.

/*
 * Copyright (C) by
 *   MetraLabs GmbH (MLAB), GERMANY
 * All rights reserved.
 *
 * Redistribution and modification of this code is strictly prohibited.
 *
 * IN NO EVENT SHALL "MLAB" 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 "MLABS" HS BEEN
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" SPECIFICALLY DISCLAIMS 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" HAS NO OBLIGATION TO PROVIDE MAINTENANCE,
 * SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#ifndef _MLAB_CURVE_H_
#define _MLAB_CURVE_H_

#include <geometry/Point.h>
#include <transform/Pose.h>
#include <image/Img.h>
#include <math/IsApprox.h>

namespace mira { namespace localization {


class Curve : public std::vector<Point2f>
{
public:

    Curve() :
        std::vector<Point2f>()
    {}

    Curve(const Curve& c) :
        std::vector<Point2f>(c)
    {}

    template<typename Reflector>
    void reflect(Reflector& r)
    {
        MIRA_REFLECT_BASE(r, std::vector<Point2f>);
    }

    void transform(const Pose2& pose);

    float distToPoint(const Point2f& p) const;

    // skip specifies the number of points to skip (4 means, take every 4th point only)
    size_t closestPoint(const Point2f& p, size_t skip = 4) const;

    double matchWithCurve(const Curve& c, float maxDist = std::numeric_limits<float>::max(),
                          float coverage = 0.0f, size_t skip = 4, const Pose2& tf = Pose2(0, 0, 0));

    void draw(Img8U3& img, Color::RGB col, float scale, int x0, int y0) const;

    void merge(const Curve& c) {
        foreach(const Point2f& p, c)
            push_back(p);
    }
};


inline void Curve::transform(const Pose2& pose)
{
    const Eigen::Matrix2f R = pose.r.toRotationMatrix();
    const Eigen::Vector2f& t = pose.t;

    for(size_t i = 0; i < size(); ++i)
    {
        Point2f& p = operator[](i);
        p = R * p + t;
    }
}

inline float Curve::distToPoint(const Point2f& p) const
{
    const Point2f& c = at(closestPoint(p));
    return (p - c).norm();
}

inline size_t Curve::closestPoint(const Point2f& p, size_t skip) const
{
    if(empty())
        MIRA_THROW(XRuntime, "Curve has no points");

    float min_sqdist = std::numeric_limits<float>::max();
    size_t best = 0;
    for (size_t i = 0; i < size(); i += skip)
    {
        const float sqd = (operator[](i) - p).squaredNorm();
        if (sqd < min_sqdist) {
            min_sqdist = sqd;
            best = i;
        }
    }

    return best;
}

inline double Curve::matchWithCurve(const Curve& pC, float maxDist,
                                    float coverage, size_t skip, const Pose2& tf)
{
    if(pC.empty())
        return std::numeric_limits<double>::max(); // same logic as below

    const float maxDistSq = maxDist * maxDist;
    const bool applyTf = !(tf.t.isZero() && isApprox<float, float>(tf.phi(), 0.0f, std::numeric_limits<float>::epsilon()));
    const Eigen::Matrix2f tfR = tf.r.toRotationMatrix();
    const Eigen::Vector2f& tfT = tf.t;

    float error = 0.0;
    size_t count = 0;
    for (size_t i = 0; i < size(); i += skip)
    {
        const Point2f xt(applyTf ? (tfR * at(i) + tfT) : at(i));
        const Point2f& y = pC.at(pC.closestPoint(xt, skip));

        const float d = (xt - y).squaredNorm();

        // don't take point if its distance is above the limit
        if (d >= maxDistSq)
            continue;

        error += d;
        ++count;
    }

    if (count < static_cast<size_t>(((size() / skip) * coverage)))
        return std::numeric_limits<double>::max();

    return (error / static_cast<float>(count));
}

inline void Curve::draw(Img8U3& img, Color::RGB col, float scale, int x0, int y0) const
{
    for (size_t i = 0; i < size(); i++)
    {
        const Point2f& point = at(i);
        int x = x0 + (int) (scale * point.x() + 0.5);
        int y = y0 - (int) (scale * point.y() + 0.5);
        if (x>0 && x<img.width() && y>0 && y<img.height())
            img(x, y) = col;
    }
}


}

template<>
class IsCollection<localization::Curve> : public std::true_type {};

}

#endif