QPInitSolver.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */

 
#pragma once
 
#include <gtsam_unstable/linear/LPInitSolver.h>
 
namespace gtsam {

class QPInitSolver {
    const QP& qp_;
public:
    QPInitSolver(const QP& qp) : qp_(qp) {}

    VectorValues solve() const {
      // Make an LP with any linear cost function. It doesn't matter for
      // initialization.
      LP initProblem;
      // make an unrelated key for a random variable cost
      Key newKey = maxKey(qp_) + 1;
      initProblem.cost = LinearCost(newKey, Vector::Ones(1));
      initProblem.equalities = qp_.equalities;
      initProblem.inequalities = qp_.inequalities;
      LPInitSolver initSolver(initProblem);
      return initSolver.solve();
    }
};
 
 
}