vpQuadProg.h

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Quadratic Programming
 *
 * Authors:
 * Olivier Kermorgant
 *
 *****************************************************************************/

#ifndef vpQuadProgh
#define vpQuadProgh

#include <vector>
#include <stdlib.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpMatrixException.h>
#include <visp3/core/vpLinProg.h>

class VISP_EXPORT vpQuadProg
{
public:
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)

  bool solveQPe(const vpMatrix &Q, const vpColVector &r,
                vpColVector &x, const double &tol = 1e-6) const;

  bool solveQPi(const vpMatrix &Q, const vpColVector &r,
                const vpMatrix &C, const vpColVector &d,
                vpColVector &x,
                bool use_equality = false,
                const double &tol = 1e-6);

  bool solveQP(const vpMatrix &Q, const vpColVector &r,
               vpMatrix A, vpColVector b,
               const vpMatrix &C, const vpColVector &d,
               vpColVector &x, const double &tol = 1e-6);

  bool setEqualityConstraint(const vpMatrix &A, const vpColVector &b, const double &tol = 1e-6);
  void resetActiveSet()
  {
    active.clear();
  }

  static void fromCanonicalCost(const vpMatrix &H, const vpColVector &c, vpMatrix &Q, vpColVector &r, const double &tol = 1e-6);
  static bool solveQPe(const vpMatrix &Q, const vpColVector &r,
                vpMatrix A, vpColVector b,
                vpColVector &x, const double &tol = 1e-6);

protected:
  std::vector<unsigned int> active;
  std::vector<unsigned int> inactive;
  vpColVector x1;
  vpMatrix Z;

  static vpColVector solveSVDorQR(const vpMatrix &A, const vpColVector &b);

  static bool solveByProjection(const vpMatrix &Q, const vpColVector &r,
                                vpMatrix &A, vpColVector &b,
                                vpColVector &x, const double &tol = 1e-6);

  static unsigned int checkDimensions(const vpMatrix &Q, const vpColVector &r,
                                      const vpMatrix* A, const vpColVector* b,
                                      const vpMatrix* C, const vpColVector* d,
                                      const std::string fct)
  {
    // check data consistency
    unsigned int n = Q.getCols();
    const bool Ab = (A != nullptr && b != nullptr && A->getRows());
    const bool Cd = (C != nullptr && d != nullptr && C->getRows());

    if (  (Ab && n != A->getCols()) ||
          (Cd && n != C->getCols()) ||
          (Ab && A->getRows() != b->getRows()) ||
          (Cd && C->getRows() != d->getRows()) ||
          Q.getRows() != r.getRows())
    {
      std::cout << "vpQuadProg::" << fct << ": wrong dimension\n" <<
                   "Q: " << Q.getRows() << "x" << Q.getCols() << " - r: " << r.getRows() << std::endl;
      if(Ab)
        std::cout << "A: " << A->getRows() << "x" << A->getCols() << " - b: " << b->getRows() << std::endl;
      if(Cd)
        std::cout << "C: " << C->getRows() << "x" << C->getCols() << " - d: " << d->getRows() << std::endl;
      throw vpMatrixException::dimensionError;
    }
    return n;
  }
#endif
};
#endif