QuadraticProgramming.hpp

Go to the documentation of this file.

#ifndef SPIDAR_QUADRATIC_PROGRAMMING_HPP
#define SPIDAR_QUADRATIC_PROGRAMMING_HPP

#include <cstdint>

#include "GaussianElimination.hpp"

namespace Spidar
{
namespace Math
{
template <class Traits>
class QuadraticProgramming
{
public:
    typedef typename Traits::ValueType      ValueType;
    typedef typename Traits::MatrixType     MatrixType;
    typedef typename Traits::VectorType     VectorType;
    typedef typename Traits::ActiveSetType  ActiveSetType;

    static const int size(void) {    return Traits::SIZE;  }

    // コンストラクタ
    QuadraticProgramming(void)
    {
        Identity(identity_);
    }

    //
    // QP Main Function
    // q: 目的関数の2次の係数行列
    // c: 目的関数の1次の係数ベクトル
    // x: 解 min < x < max
    // min: xの最小値
    // max: xの最大値
    int solve(const MatrixType& q, const VectorType& c, VectorType& x, const VectorType& min, const VectorType& max)
    {
        // 初期化
        initialize(x, min, max);

        // 繰り返し
        for (int i=0; i<64; ++i)
        {
            updateMatrix(q, c);

            GaussianElimination::solve(matR_, vecL_, vecX_);

            checkResult();

            if (x==vecX_)
            {
                if(calcLambda())    return i;
            }
            else
            {
                calcAlpha(x);
            }
        }
        return -1;
    }

private:

    //
    void initialize(VectorType& x, const VectorType& min, const VectorType& max)
    {
        minX_ = VectorType() - min;
        maxX_ = max;

        for (int i=0; i<size(); ++i)
        {
            x[i] = minX_[i];
            activeSet_[i] = -1;
        }
    }

    //
    void updateMatrix(const MatrixType& q, const VectorType& c)
    {
        for (int i=0; i<size(); ++i)
        {
            vecL_[i] = c[i];

            for (int j=0; j<size(); ++j)
            {
                if (activeSet_[j] > 0)
                {
                    vecL_[i] -= q[i][j] * maxX_[i];
                    matR_[i][j] = identity_[i][j];
                }
                else if (activeSet_[j] < 0)
                {
                    vecL_[i] += q[i][j] * minX_[i];
                    matR_[i][j] = -identity_[i][j];
                }
                else
                {
                    matR_[i][j] = q[i][j];
                }
            }
        }
    }

    void checkResult(void)
    {
        for (int i=0; i<size(); ++i)
        {
            if (activeSet_[i] > 0)
            {
                vecY_[i] = vecX_[i];
                vecX_[i] = maxX_[i];
            }
            else if (activeSet_[i] < 0)
            {
                vecY_[i] = vecX_[i];
                vecX_[i] = -minX_[i];
            }
            else // activeSet_[i]==0
            {
                vecY_[i] = ValueType(0);
            }
        }
    }

    //
    bool calcLambda(void)
    {
        bool bval = true;

        for (int i=0; i<size(); ++i)
        {
            if (activeSet_[i]!=0)
            {
                if (vecY_[i]<0)
                {
                    activeSet_[i] = 0;
                    bval = false;
                }
            }
        }
        return bval;
    }

    //
    void calcAlpha(VectorType& x)
    {
        int minIndex = -1, bval;
        ValueType alpha, minAlpha = 1;

        for (int i=0; i<size(); ++i)
        {
            if (activeSet_[i]==0)
            {
                vecD_[i] = vecX_[i] - x[i];

                if (vecD_[i] < 0)
                {
                    alpha = -(minX_[i] + x[i]) / vecD_[i];

                    if (alpha > 0 && minAlpha > alpha)
                    {
                        minAlpha = alpha;
                        minIndex = i;
                        bval = -1;
                    }
                    else if (alpha <= 0)
                    {
                        activeSet_[i] = -1;
                    }
                }
                else if (vecD_[i] > 0)
                {
                    alpha = (maxX_[i] - x[i]) / vecD_[i];

                    if (alpha > 0 && minAlpha > alpha)
                    {
                        minAlpha = alpha;
                        minIndex = i;
                        bval = 1;
                    }
                    else if (alpha <= 0)
                    {
                        activeSet_[i] = 1;
                    }
                }
            }
        }

        if (minIndex >= 0)
        {
            activeSet_[minIndex] = bval;

            for (int i=0; i<size(); ++i)
            {
                if (!activeSet_[i]) 
                {
                    x[i] += minAlpha * vecD_[i];
                }
            }
        }
        else
        {
            for (int i=0; i<size(); ++i)
            {
                x[i] = vecX_[i];
            }
        }
    }

    MatrixType      identity_;
    VectorType      minX_;              
    VectorType      maxX_;

    MatrixType      matR_;
    VectorType      vecL_;

    VectorType      vecX_;
    VectorType      vecY_;
    VectorType      vecD_;
    ActiveSetType   activeSet_;

}; // end of class QuadraticProgramming.

} // end of namespace Math.
} // end of namespace Spidar.

#endif // SPIDAR_QUADRATIC_PROGRAMMING_HPP

// end of file.