GteApprOrthogonalLine2.h

Go to the documentation of this file.

// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2017
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// File Version: 3.0.0 (2016/06/19)

#pragma once

#include <Mathematics/GteVector2.h>
#include <Mathematics/GteApprQuery.h>
#include <Mathematics/GteLine.h>
#include <Mathematics/GteSymmetricEigensolver2x2.h>

// Least-squares fit of a line to (x,y) data by using distance measurements
// orthogonal to the proposed line.  The return value is 'true' iff the fit
// is unique (always successful, 'true' when a minimum eigenvalue is unique).
// The mParameters value is a line with (P,D) = (origin,direction).  The
// error for S = (x0,y0) is (S-P)^T*(I - D*D^T)*(S-P).

namespace gte
{

template <typename Real>
class ApprOrthogonalLine2
    :
    public ApprQuery<Real, ApprOrthogonalLine2<Real>, Vector2<Real>>
{
public:
    // Initialize the model parameters to zero.
    ApprOrthogonalLine2();

    // Basic fitting algorithm.
    bool Fit(int numPoints, Vector2<Real> const* points);
    Line2<Real> const& GetParameters() const;

    // Functions called by ApprQuery::RANSAC.  See GteApprQuery.h for a
    // detailed description.
    int GetMinimumRequired() const;
    Real Error(Vector2<Real> const& observation) const;
    bool Fit(std::vector<Vector2<Real>> const& observations,
        std::vector<int> const& indices);

private:
    Line2<Real> mParameters;
};


template <typename Real>
ApprOrthogonalLine2<Real>::ApprOrthogonalLine2()
    :
    mParameters(Vector2<Real>::Zero(), Vector2<Real>::Zero())
{
}

template <typename Real>
bool ApprOrthogonalLine2<Real>::Fit(int numPoints,
    Vector2<Real> const* points)
{
    if (numPoints >= GetMinimumRequired() && points)
    {
        // Compute the mean of the points.
        Vector2<Real> mean = Vector2<Real>::Zero();
        for (int i = 0; i < numPoints; ++i)
        {
            mean += points[i];
        }
        mean /= (Real)numPoints;

        // Compute the covariance matrix of the points.
        Real covar00 = (Real)0, covar01 = (Real)0, covar11 = (Real)0;
        for (int i = 0; i < numPoints; ++i)
        {
            Vector2<Real> diff = points[i] - mean;
            covar00 += diff[0] * diff[0];
            covar01 += diff[0] * diff[1];
            covar11 += diff[1] * diff[1];
        }

        // Solve the eigensystem.
        SymmetricEigensolver2x2<Real> es;
        std::array<Real, 2> eval;
        std::array<std::array<Real, 2>, 2> evec;
        es(covar00, covar01, covar11, +1, eval, evec);

        // The line direction is the eigenvector in the direction of largest
        // variance of the points.
        mParameters.origin = mean;
        mParameters.direction = evec[1];

        // The fitted line is unique when the maximum eigenvalue has
        // multiplicity 1.
        return eval[0] < eval[1];
    }

    mParameters = Line2<Real>(Vector2<Real>::Zero(), Vector2<Real>::Zero());
    return false;
}

template <typename Real>
Line2<Real> const& ApprOrthogonalLine2<Real>::GetParameters() const
{
    return mParameters;
}

template <typename Real>
int ApprOrthogonalLine2<Real>::GetMinimumRequired() const
{
    return 2;
}

template <typename Real>
Real ApprOrthogonalLine2<Real>::Error(Vector2<Real> const& observation) const
{
    Vector2<Real> diff = observation - mParameters.origin;
    Real sqrlen = Dot(diff, diff);
    Real dot = Dot(diff, mParameters.direction);
    Real error = std::abs(sqrlen - dot*dot);
    return error;
}

template <typename Real>
bool ApprOrthogonalLine2<Real>::Fit(
    std::vector<Vector2<Real>> const& observations,
    std::vector<int> const& indices)
{
    if (static_cast<int>(indices.size()) >= GetMinimumRequired())
    {
        // Compute the mean of the points.
        Vector2<Real> mean = Vector2<Real>::Zero();
        for (auto index : indices)
        {
            mean += observations[index];
        }
        mean /= (Real)indices.size();

        // Compute the covariance matrix of the points.
        Real covar00 = (Real)0, covar01 = (Real)0, covar11 = (Real)0;
        for (auto index : indices)
        {
            Vector2<Real> diff = observations[index] - mean;
            covar00 += diff[0] * diff[0];
            covar01 += diff[0] * diff[1];
            covar11 += diff[1] * diff[1];
        }

        // Solve the eigensystem.
        // Solve the eigensystem.
        SymmetricEigensolver2x2<Real> es;
        std::array<Real, 2> eval;
        std::array<std::array<Real, 2>, 2> evec;
        es(covar00, covar01, covar11, +1, eval, evec);

        // The line direction is the eigenvector in the direction of largest
        // variance of the points.
        mParameters.origin = mean;
        mParameters.direction = evec[1];

        // The fitted line is unique when the maximum eigenvalue has
        // multiplicity 1.
        return eval[0] < eval[1];
    }

    mParameters = Line2<Real>(Vector2<Real>::Zero(), Vector2<Real>::Zero());
    return false;
}


}