comments
parent
d8b658e2ea
commit
49cc1e5f1f
|
@ -22,18 +22,16 @@
|
||||||
#include <gtsam_unstable/linear/LinearEquality.h>
|
#include <gtsam_unstable/linear/LinearEquality.h>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* This class is used to represent an equality constraint on
|
* Collection of all Linear Equality constraints Ax=b of
|
||||||
* a Programming problem of the form Ax = b.
|
* a Programming problem as a Factor Graph
|
||||||
*/
|
*/
|
||||||
class EqualityFactorGraph: public FactorGraph<LinearEquality> {
|
class EqualityFactorGraph: public FactorGraph<LinearEquality> {
|
||||||
public:
|
public:
|
||||||
typedef boost::shared_ptr<EqualityFactorGraph> shared_ptr;
|
typedef boost::shared_ptr<EqualityFactorGraph> shared_ptr;
|
||||||
|
|
||||||
/** compute error of a guess.
|
/// Compute error of a guess.
|
||||||
* TODO: This code is duplicated in GaussianFactorGraph and NonlinearFactorGraph!!
|
|
||||||
* Remove it!
|
|
||||||
*/
|
|
||||||
double error(const VectorValues& x) const {
|
double error(const VectorValues& x) const {
|
||||||
double total_error = 0.;
|
double total_error = 0.;
|
||||||
for (const sharedFactor& factor : *this) {
|
for (const sharedFactor& factor : *this) {
|
||||||
|
|
|
@ -25,6 +25,10 @@
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Collection of all Linear Inequality constraints Ax-b <= 0 of
|
||||||
|
* a Programming problem as a Factor Graph
|
||||||
|
*/
|
||||||
class InequalityFactorGraph: public FactorGraph<LinearInequality> {
|
class InequalityFactorGraph: public FactorGraph<LinearInequality> {
|
||||||
private:
|
private:
|
||||||
typedef FactorGraph<LinearInequality> Base;
|
typedef FactorGraph<LinearInequality> Base;
|
||||||
|
|
|
@ -14,6 +14,7 @@
|
||||||
* @brief LinearInequality derived from Base with constrained noise model
|
* @brief LinearInequality derived from Base with constrained noise model
|
||||||
* @date Nov 27, 2014
|
* @date Nov 27, 2014
|
||||||
* @author Duy-Nguyen Ta
|
* @author Duy-Nguyen Ta
|
||||||
|
* @author Ivan Dario Jimenez
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
@ -26,7 +27,7 @@ namespace gtsam {
|
||||||
typedef Eigen::RowVectorXd RowVector;
|
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
|
* inheriting JacobianFactor with the special Constrained noise model
|
||||||
*/
|
*/
|
||||||
class LinearInequality: public JacobianFactor {
|
class LinearInequality: public JacobianFactor {
|
||||||
|
|
|
@ -237,23 +237,21 @@ pair<QP, QP> testParser(QPSParser parser) {
|
||||||
|
|
||||||
TEST(QPSolver, ParserSyntaticTest) {
|
TEST(QPSolver, ParserSyntaticTest) {
|
||||||
auto expectedActual = testParser(QPSParser("QPExample.QPS"));
|
auto expectedActual = testParser(QPSParser("QPExample.QPS"));
|
||||||
CHECK(
|
CHECK(assert_equal(expectedActual.first.cost, expectedActual.second.cost,
|
||||||
assert_equal(expectedActual.first.cost, expectedActual.second.cost, 1e-7));
|
1e-7));
|
||||||
CHECK(
|
CHECK(assert_equal(expectedActual.first.inequalities,
|
||||||
assert_equal(expectedActual.first.inequalities,
|
|
||||||
expectedActual.second.inequalities, 1e-7));
|
expectedActual.second.inequalities, 1e-7));
|
||||||
CHECK(
|
CHECK(assert_equal(expectedActual.first.equalities,
|
||||||
assert_equal(expectedActual.first.equalities,
|
|
||||||
expectedActual.second.equalities, 1e-7));
|
expectedActual.second.equalities, 1e-7));
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(QPSolver, ParserSemanticTest) {
|
TEST(QPSolver, ParserSemanticTest) {
|
||||||
auto expected_actual = testParser(QPSParser("QPExample.QPS"));
|
auto expected_actual = testParser(QPSParser("QPExample.QPS"));
|
||||||
VectorValues actualSolution, expectedSolution;
|
VectorValues actualSolution, expectedSolution;
|
||||||
boost::tie(expectedSolution, boost::tuples::ignore) = QPSolver(
|
boost::tie(expectedSolution, boost::tuples::ignore) =
|
||||||
expected_actual.first).optimize();
|
QPSolver(expected_actual.first).optimize();
|
||||||
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(
|
boost::tie(actualSolution, boost::tuples::ignore) =
|
||||||
expected_actual.second).optimize();
|
QPSolver(expected_actual.second).optimize();
|
||||||
CHECK(assert_equal(actualSolution, expectedSolution, 1e-7));
|
CHECK(assert_equal(actualSolution, expectedSolution, 1e-7));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue