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

[BUGFIX-WARNING] Fixed warning in gcc5 by adding SYSTEM to gtsam included eigen.

release/4.3a0
Ivan Jimenez 2016-02-27 19:21:42 -05:00
parent 28f7704eea
commit 6cc4ab8bdf
8 changed files with 75 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -1,3 +1,4 @@
/gtsam/inference/FactorGraph.h
/build*
.idea
*.pyc

2
CMakeLists.txt
View File

@ -241,7 +241,7 @@ else()
endif()
# Add the bundled version of eigen to the include path so that it can still be included
# with #include <Eigen/Core>
include_directories(BEFORE "gtsam/3rdparty/Eigen/")
include_directories(BEFORE SYSTEM "gtsam/3rdparty/Eigen/")
# set full path to be used by external projects
# this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in

2
gtsam/inference/FactorGraph.h
View File

@ -351,3 +351,5 @@ namespace gtsam {
}; // FactorGraph
} // namespace gtsam
#include <gtsam/inference/FactorGraph-inst.h>

4
gtsam_unstable/linear/QP.h
View File

@ -45,6 +45,10 @@ struct QP {
_linearInequalities) {
}
QP(std::string MPS_FileName):
cost(), equalities(), inequalities() {}
/** print */
void print(const std::string& s = "") {
std::cout << s << std::endl;

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

@ -213,6 +213,41 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) {
CHECK(assert_equal(expectedSolution, solution, 1e-100));
}
QP createExampleQP(){
QP exampleqp;
exampleqp.cost.push_back(
HessianFactor(X(1),Y(1),
8.0 *ones(1,1), 2.0 * ones(1,1), 1.5*ones(1),
10.0 *ones(1,1), -2.0 *ones(1), 4.0));
// 2x + y >= 2
exampleqp.inequalities.push_back(
LinearInequality(X(1), -2.0*ones(1,1), Y(1), -ones(1,1), -2, 0));
// -x + 2y <= 6
exampleqp.inequalities.push_back(
LinearInequality(X(1), -ones(1,1), Y(1), 2.0* ones(1,1), 6, 1));
//x >= 0
exampleqp.inequalities.push_back(
LinearInequality(X(1), -ones(1,1), 0, 2));
// y > = 0
exampleqp.inequalities.push_back(
LinearInequality(Y(1), -ones(1,1), 0, 3));
// x<= 20
exampleqp.inequalities.push_back(
LinearInequality(X(1), ones(1,1), 20, 4));
return exampleqp;
};
TEST(QPSolver, QPExampleData){
QP exampleqp("QPExample.QPS");
QP expectedqp = createExampleQP();
// min f(x,y) = 4 + 1.5x -y + 0.5(8x^2 + 2xy + 2yx + 10y^2
// CHECK(expectedqp.cost.equals(exampleqp.cost, 1e-7));
// CHECK(expectedqp.inequalities.equals(exampleqp.inequalities, 1e-7));
// CHECK(expectedqp.equalities.equals(exampleqp.equalities, 1e-7));
}
/* ************************************************************************* */
// Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html
QP createTestMatlabQPEx() {

2
gtsam_unstable/nonlinear/ConstrainedFactor.h
View File

@ -40,6 +40,8 @@ public:
typedef boost::shared_ptr<ConstrainedFactor> shared_ptr;
public:
/// Default constructor with default key value.
ConstrainedFactor() : dualKey_(0) {}
/// Construct with dual key
ConstrainedFactor(Key dualKey) : dualKey_(dualKey) {}

2
gtsam_unstable/nonlinear/NonlinearConstraint.h
View File

@ -327,7 +327,7 @@ public:
/**
* Default Constructor for I/O
*/
NonlinearConstraint3() {
NonlinearConstraint3(){
}
/**

48
gtsam_unstable/nonlinear/tests/testLinearConstraintSQP.cpp
View File

@ -38,18 +38,22 @@ const double tol = 1e-10;
//******************************************************************************
// x + y - 1 = 0
class ConstraintProblem1 : public LinearEqualityFactor2<double, double> {
class ConstraintProblem1: public LinearEqualityFactor2<double, double> {
typedef LinearEqualityFactor2<double, double> Base;
public:
ConstraintProblem1(Key xK, Key yK, Key dualKey) : Base(xK, yK, dualKey, 1) {}
ConstraintProblem1(Key xK, Key yK, Key dualKey) :
Base(xK, yK, dualKey, 1) {
}
// x + y - 1
Vector evaluateError(const double& x, const double& y,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
if (H1) *H1 = eye(1);
if (H2) *H2 = eye(1);
if (H1)
*H1 = eye(1);
if (H2)
*H2 = eye(1);
return (Vector(1) << x + y - 1.0).finished();
}
};
@ -62,10 +66,10 @@ TEST(testlcnlpSolver, QPProblem) {
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence here we have G11 = 2, G12 = 0, G22 = 2, g1 = 0, g2 = 0, f = 0
HessianFactor hf(X(1), Y(1), 2.0 * ones(1,1), zero(1), zero(1),
2*ones(1,1), zero(1) , 0);
2*ones(1,1), zero(1) , 0);
EqualityFactorGraph equalities;
LinearEquality linearConstraint(X(1), ones(1), Y(1), ones(1), 1*ones(1), dualKey); // x + y - 1 = 0
LinearEquality linearConstraint(X(1), ones(1), Y(1), ones(1), 1*ones(1), dualKey);// x + y - 1 = 0
equalities.push_back(linearConstraint);
// Compare against QP
@ -86,7 +90,7 @@ TEST(testlcnlpSolver, QPProblem) {
Values linPoint;
linPoint.insert<Vector1>(X(1), zero(1));
linPoint.insert<Vector1>(Y(1), zero(1));
lcnlp.cost.add(LinearContainerFactor(hf, linPoint)); // wrap it using linearcontainerfactor
lcnlp.cost.add(LinearContainerFactor(hf, linPoint));// wrap it using linearcontainerfactor
lcnlp.linearEqualities.add(ConstraintProblem1(X(1), Y(1), dualKey));
Values initialValues;
@ -106,15 +110,18 @@ TEST(testlcnlpSolver, QPProblem) {
/**
* A simple linear constraint on Pose3's x coordinate enforcing x==0
*/
class LineConstraintX : public LinearEqualityFactor1<Pose3> {
class LineConstraintX: public LinearEqualityFactor1<Pose3> {
typedef LinearEqualityFactor1<Pose3> Base;
public:
LineConstraintX(Key key, Key dualKey) : Base(key, dualKey, 1) {
LineConstraintX(Key key, Key dualKey) :
Base(key, dualKey, 1) {
}
Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H = boost::none) const {
Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const {
if (H)
*H = (Matrix(1,6) << zeros(1,3), pose.rotation().matrix().row(0)).finished();
*H =
(Matrix(1, 6) << zeros(1, 3), pose.rotation().matrix().row(0)).finished();
return (Vector(1) << pose.x()).finished();
}
};
@ -122,7 +129,6 @@ public:
TEST(testlcnlpSolver, poseOnALine) {
const Key dualKey = 0;
//Instantiate LinearConstraintNLP
LinearConstraintNLP lcnlp;
lcnlp.cost.add(PriorFactor<Pose3>(X(1), Pose3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 0, 0)), noiseModel::Unit::Create(6)));
@ -144,16 +150,20 @@ TEST(testlcnlpSolver, poseOnALine) {
//******************************************************************************
/// x + y - 1 <= 0
class InequalityProblem1 : public LinearInequalityFactor2<double, double> {
class InequalityProblem1: public LinearInequalityFactor2<double, double> {
typedef LinearInequalityFactor2<double, double> Base;
public:
InequalityProblem1(Key xK, Key yK, Key dualKey) : Base(xK, yK, dualKey) {}
InequalityProblem1(Key xK, Key yK, Key dualKey) :
Base(xK, yK, dualKey) {
}
double computeError(const double& x, const double& y,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
if (H1) *H1 = eye(1);
if (H2) *H2 = eye(1);
if (H1)
*H1 = eye(1);
if (H2)
*H2 = eye(1);
return x + y - 1.0;
}
};
@ -166,10 +176,10 @@ TEST(testlcnlpSolver, inequalityConstraint) {
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence here we have G11 = 2, G12 = 0, G22 = 2, g1 = 0, g2 = 0, f = 0
HessianFactor hf(X(1), Y(1), 2.0 * ones(1,1), zero(1), zero(1),
2*ones(1,1), zero(1) , 0);
2*ones(1,1), zero(1) , 0);
InequalityFactorGraph inequalities;
LinearInequality linearConstraint(X(1), ones(1), Y(1), ones(1), 1.0, dualKey); // x + y - 1 <= 0
LinearInequality linearConstraint(X(1), ones(1), Y(1), ones(1), 1.0, dualKey);// x + y - 1 <= 0
inequalities.push_back(linearConstraint);
// Compare against QP
@ -190,7 +200,7 @@ TEST(testlcnlpSolver, inequalityConstraint) {
Values linPoint;
linPoint.insert<Vector1>(X(1), zero(1));
linPoint.insert<Vector1>(Y(1), zero(1));
lcnlp.cost.add(LinearContainerFactor(hf, linPoint)); // wrap it using linearcontainerfactor
lcnlp.cost.add(LinearContainerFactor(hf, linPoint));// wrap it using linearcontainerfactor
lcnlp.linearInequalities.add(InequalityProblem1(X(1), Y(1), dualKey));
Values initialValues;