12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/feature/LPSolver' into feature/LPSolver

release/4.3a0
= 2016-06-18 09:15:39 -04:00
parent f3e756805d aa443fdef0
commit 0092c27551
5 changed files with 31 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam_unstable/linear/EqualityFactorGraph.h
View File

@ -22,18 +22,16 @@
#include <gtsam_unstable/linear/LinearEquality.h>
namespace gtsam {
/**
* This class is used to represent an equality constraint on
* a Programming problem of the form Ax = b.
* Collection of all Linear Equality constraints Ax=b of
* a Programming problem as a Factor Graph
*/
class EqualityFactorGraph: public FactorGraph<LinearEquality> {
public:
typedef boost::shared_ptr<EqualityFactorGraph> shared_ptr;
/** compute error of a guess.
* TODO: This code is duplicated in GaussianFactorGraph and NonlinearFactorGraph!!
* Remove it!
*/
/// Compute error of a guess.
double error(const VectorValues& x) const {
double total_error = 0.;
for (const sharedFactor& factor : *this) {

4
gtsam_unstable/linear/InequalityFactorGraph.h
View File

@ -25,6 +25,10 @@
namespace gtsam {
/**
* Collection of all Linear Inequality constraints Ax-b <= 0 of
* a Programming problem as a Factor Graph
*/
class InequalityFactorGraph: public FactorGraph<LinearInequality> {
private:
typedef FactorGraph<LinearInequality> Base;

3
gtsam_unstable/linear/LinearInequality.h
View File

@ -14,6 +14,7 @@
* @brief LinearInequality derived from Base with constrained noise model
* @date Nov 27, 2014
* @author Duy-Nguyen Ta
* @author Ivan Dario Jimenez
*/
#pragma once
@ -26,7 +27,7 @@ namespace gtsam {
typedef Eigen::RowVectorXd RowVector;
/**
* This class defines a linear inequality constraint g(x)<=0,
* This class defines a linear inequality constraint Ax-b <= 0,
* inheriting JacobianFactor with the special Constrained noise model
*/
class LinearInequality: public JacobianFactor {

18
gtsam_unstable/linear/RawQP.cpp
View File

@ -225,31 +225,31 @@ QP RawQP::makeQP() {
madeQP.cost.push_back(obj);
for (auto kv : E) {
std::vector < std::pair<Key, Matrix11> > KeyMatPair;
std::map<Key, Matrix11> keyMatrixMap;
for (auto km : kv.second) {
KeyMatPair.push_back(km);
keyMatrixMap.insert(km);
}
madeQP.equalities.push_back(
LinearEquality(KeyMatPair, b[kv.first] * I_1x1, dual_key_num++));
LinearEquality(keyMatrixMap, b[kv.first] * I_1x1, dual_key_num++));
}
for (auto kv : IG) {
std::vector < std::pair<Key, Matrix11> > KeyMatPair;
std::map<Key, Matrix11> keyMatrixMap;
for (auto km : kv.second) {
km.second = -km.second;
KeyMatPair.push_back(km);
keyMatrixMap.insert(km);
}
madeQP.inequalities.push_back(
LinearInequality(KeyMatPair, -b[kv.first], dual_key_num++));
LinearInequality(keyMatrixMap, -b[kv.first], dual_key_num++));
}
for (auto kv : IL) {
std::vector < std::pair<Key, Matrix11> > KeyMatPair;
std::map<Key, Matrix11> keyMatrixMap;
for (auto km : kv.second) {
KeyMatPair.push_back(km);
keyMatrixMap.insert(km);
}
madeQP.inequalities.push_back(
LinearInequality(KeyMatPair, b[kv.first], dual_key_num++));
LinearInequality(keyMatrixMap, b[kv.first], dual_key_num++));
}
for (Key k : keys) {

26
gtsam_unstable/linear/tests/testQPSolver.cpp
View File

@ -222,9 +222,9 @@ pair<QP, QP> testParser(QPSParser parser) {
// 2x + y >= 2
// -x + 2y <= 6
expectedqp.inequalities.push_back(
LinearInequality(X2, -I_1x1, X1, -2.0 * I_1x1, -2, 0));
LinearInequality(X1, -2.0 * I_1x1, X2, -I_1x1, -2, 0));
expectedqp.inequalities.push_back(
LinearInequality(X2, 2.0 * I_1x1, X1, -I_1x1, 6, 1));
LinearInequality(X1, -I_1x1, X2, 2.0 * I_1x1, 6, 1));
// x<= 20
expectedqp.inequalities.push_back(LinearInequality(X1, I_1x1, 20, 4));
//x >= 0
@ -237,23 +237,21 @@ pair<QP, QP> testParser(QPSParser parser) {
TEST(QPSolver, ParserSyntaticTest) {
auto expectedActual = testParser(QPSParser("QPExample.QPS"));
CHECK(
assert_equal(expectedActual.first.cost, expectedActual.second.cost, 1e-7));
CHECK(
assert_equal(expectedActual.first.inequalities,
expectedActual.second.inequalities, 1e-7));
CHECK(
assert_equal(expectedActual.first.equalities,
expectedActual.second.equalities, 1e-7));
CHECK(assert_equal(expectedActual.first.cost, expectedActual.second.cost,
1e-7));
CHECK(assert_equal(expectedActual.first.inequalities,
expectedActual.second.inequalities, 1e-7));
CHECK(assert_equal(expectedActual.first.equalities,
expectedActual.second.equalities, 1e-7));
}
TEST(QPSolver, ParserSemanticTest) {
auto expected_actual = testParser(QPSParser("QPExample.QPS"));
VectorValues actualSolution, expectedSolution;
boost::tie(expectedSolution, boost::tuples::ignore) = QPSolver(
expected_actual.first).optimize();
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(
expected_actual.second).optimize();
boost::tie(expectedSolution, boost::tuples::ignore) =
QPSolver(expected_actual.first).optimize();
boost::tie(actualSolution, boost::tuples::ignore) =
QPSolver(expected_actual.second).optimize();
CHECK(assert_equal(actualSolution, expectedSolution, 1e-7));
}