LPSolver.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/LP.h>
#include <gtsam_unstable/linear/ActiveSetSolver.h>
#include <gtsam_unstable/linear/LPInitSolver.h>

#include <limits>
#include <algorithm>

namespace gtsam {

struct LPPolicy {
  static constexpr double maxAlpha = std::numeric_limits<double>::infinity();

  static GaussianFactorGraph buildCostFunction(const LP& lp,
                                               const VectorValues& xk) {
    GaussianFactorGraph graph;
    for (LinearCost::const_iterator it = lp.cost.begin(); it != lp.cost.end();
         ++it) {
      size_t dim = lp.cost.getDim(it);
      Vector b = xk.at(*it) - lp.cost.getA(it).transpose();  // b = xk-g
      graph.emplace_shared<JacobianFactor>(*it, Matrix::Identity(dim, dim), b);
    }

    KeySet allKeys = lp.inequalities.keys();
    allKeys.merge(lp.equalities.keys());
    allKeys.merge(KeySet(lp.cost.keys()));
    // Add corresponding factors for all variables that are not explicitly in
    // the cost function. Gradients of the cost function wrt to these variables
    // are zero (g=0), so b=xk
    if (lp.cost.keys().size() != allKeys.size()) {
      KeySet difference;
      std::set_difference(allKeys.begin(), allKeys.end(), lp.cost.begin(),
                          lp.cost.end(),
                          std::inserter(difference, difference.end()));
      for (Key k : difference) {
        size_t dim = lp.constrainedKeyDimMap().at(k);
        graph.emplace_shared<JacobianFactor>(k, Matrix::Identity(dim, dim), xk.at(k));
      }
    }
    return graph;
  }
};

using LPSolver = ActiveSetSolver<LP, LPPolicy, LPInitSolver>;

}