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/config.h>
 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
 
#include <gtsam_unstable/linear/LPInitSolver.h>
#include <gtsam_unstable/linear/QP.h>
 
#include <algorithm>
 
namespace gtsam {
namespace internal {
 
/* ************************************************************************* */
inline Key maxKey(const QP& qp) {
  Key maxKey = 0;
  bool initialized = false;
  auto update = [&](const KeySet& keys) {
    if (!keys.empty()) {
      maxKey = initialized ? std::max(maxKey, *keys.rbegin()) : *keys.rbegin();
      initialized = true;
    }
  };
  update(qp.cost.keys());
  update(qp.equalities.keys());
  update(qp.inequalities.keys());
  return maxKey;
}
 
}  // namespace internal

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 = internal::maxKey(qp_) + 1;
    initProblem.cost = LinearCost(newKey, Vector::Ones(1));
    initProblem.equalities = qp_.equalities;
    initProblem.inequalities = qp_.inequalities;
    LPInitSolver initSolver(initProblem);
    return initSolver.solve();
  }
};
 
}  // namespace gtsam
 
#endif  // GTSAM_ALLOW_DEPRECATED_SINCE_V43