vpQuadProg.h

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2023 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 https://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
 */
 
#ifndef _vpQuadProg_h_
#define _vpQuadProg_h_
 
#include <stdlib.h>
#include <vector>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpLinProg.h>
#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpMatrixException.h>
 
BEGIN_VISP_NAMESPACE
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
};
END_VISP_NAMESPACE
#endif