81 lines
2.3 KiB
C++
81 lines
2.3 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file QP.h
|
|
* @brief Factor graphs of a Quadratic Programming problem
|
|
* @date Dec 8, 2014
|
|
* @author Duy-Nguyen Ta
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam/linear/GaussianFactorGraph.h>
|
|
#include <gtsam_unstable/linear/EqualityFactorGraph.h>
|
|
#include <gtsam_unstable/linear/InequalityFactorGraph.h>
|
|
#include <gtsam/slam/dataset.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* Struct contains factor graphs of a Quadratic Programming problem
|
|
*/
|
|
struct QP {
|
|
GaussianFactorGraph cost; //!< Quadratic cost factors
|
|
EqualityFactorGraph equalities; //!< linear equality constraints: cE(x) = 0
|
|
InequalityFactorGraph inequalities; //!< linear inequality constraints: cI(x) <= 0
|
|
|
|
private:
|
|
mutable VariableIndex cachedCostVariableIndex_;
|
|
|
|
public:
|
|
/** default constructor */
|
|
QP() :
|
|
cost(), equalities(), inequalities() {
|
|
}
|
|
|
|
/** constructor */
|
|
QP(const GaussianFactorGraph& _cost,
|
|
const EqualityFactorGraph& _linearEqualities,
|
|
const InequalityFactorGraph& _linearInequalities) :
|
|
cost(_cost), equalities(_linearEqualities), inequalities(
|
|
_linearInequalities) {
|
|
}
|
|
|
|
/** print */
|
|
void print(const std::string& s = "") {
|
|
std::cout << s << std::endl;
|
|
cost.print("Quadratic cost factors: ");
|
|
equalities.print("Linear equality factors: ");
|
|
inequalities.print("Linear inequality factors: ");
|
|
}
|
|
|
|
const VariableIndex& costVariableIndex() const {
|
|
if (cachedCostVariableIndex_.size() == 0)
|
|
cachedCostVariableIndex_ = VariableIndex(cost);
|
|
return cachedCostVariableIndex_;
|
|
}
|
|
|
|
Vector costGradient(Key key, const VectorValues& delta) const {
|
|
Vector g = Vector::Zero(delta.at(key).size());
|
|
if (costVariableIndex().find(key) != costVariableIndex().end()) {
|
|
for (size_t factorIx : costVariableIndex()[key]) {
|
|
GaussianFactor::shared_ptr factor = cost.at(factorIx);
|
|
g += factor->gradient(key, delta);
|
|
}
|
|
}
|
|
return g;
|
|
}
|
|
};
|
|
|
|
} // namespace gtsam
|
|
|