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

Get rid of deprecated functions

release/4.3a0
dellaert 2016-05-06 09:40:08 -07:00
parent 272303bc90
commit 9cd6f0b066
2 changed files with 74 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam_unstable/linear/tests/testLPSolver.cpp
View File

@ -139,7 +139,7 @@ TEST(LPInitSolver, initialization) {
xy0.insert(yKey, Vector::Constant(1, y0)); xy0.insert(yKey, Vector::Constant(1, y0));
VectorValues xyInit = lpSolveInit.optimize(xy0).first; VectorValues xyInit = lpSolveInit.optimize(xy0).first;
VectorValues expected_init; VectorValues expected_init;
expected_init.insert(1, Vector2( 1, 1)); expected_init.insert(1, Vector::Ones(2));
expected_init.insert(2, Vector::Constant(1, -1)); expected_init.insert(2, Vector::Constant(1, -1));
CHECK(assert_equal(expected_init, xyInit, 1e-10)); CHECK(assert_equal(expected_init, xyInit, 1e-10));
@ -188,7 +188,7 @@ init.insert(1, Vector::Zero(2));
VectorValues x1 = lpSolver.solveWithCurrentWorkingSet(init, VectorValues x1 = lpSolver.solveWithCurrentWorkingSet(init,
InequalityFactorGraph()); InequalityFactorGraph());
VectorValues expected_x1; VectorValues expected_x1;
expected_x1.insert(1, Vector2( 1, 1)); expected_x1.insert(1, Vector::Ones(2));
CHECK(assert_equal(expected_x1, x1, 1e-10)); CHECK(assert_equal(expected_x1, x1, 1e-10));
VectorValues result, duals; VectorValues result, duals;

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

@ -28,7 +28,7 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::symbol_shorthand; using namespace gtsam::symbol_shorthand;
const Matrix One = I_1x1; static const Vector kOne = Vector::Ones(1), kZero = Vector::Zero(1);
/* ************************************************************************* */ /* ************************************************************************* */
// Create test graph according to Forst10book_pg171Ex5 // Create test graph according to Forst10book_pg171Ex5
@ -42,14 +42,14 @@ QP createTestCase() {
//TODO: THIS TEST MIGHT BE WRONG : the last parameter might be 5 instead of 10 because the form of the equation //TODO: THIS TEST MIGHT BE WRONG : the last parameter might be 5 instead of 10 because the form of the equation
// Should be 0.5x'Gx + gx + f : Nocedal 449 // Should be 0.5x'Gx + gx + f : Nocedal 449
qp.cost.push_back( qp.cost.push_back(
HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), -Matrix::Ones(1, 1), 3.0 * I_1x1, HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1,
2.0 * Matrix::Ones(1, 1), Z_1x1, 10.0)); 3.0 * I_1x1, 2.0 * I_1x1, Z_1x1, 10.0));
// Inequality constraints // Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 1)); // -x1 <= 0 qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 1)); // -x1 <= 0
qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 2)); // -x2 <= 0 qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 2)); // -x2 <= 0
qp.inequalities.push_back(LinearInequality(X(1), I_1x1, 1.5, 3)); // x1 <= 3/2 qp.inequalities.push_back(LinearInequality(X(1), I_1x1, 1.5, 3)); // x1 <= 3/2
return qp; return qp;
} }
@ -57,8 +57,8 @@ QP createTestCase() {
TEST(QPSolver, TestCase) { TEST(QPSolver, TestCase) {
VectorValues values; VectorValues values;
double x1 = 5, x2 = 7; double x1 = 5, x2 = 7;
values.insert(X(1), x1 * Matrix::Ones(1, 1)); values.insert(X(1), x1 * I_1x1);
values.insert(X(2), x2 * Matrix::Ones(1, 1)); values.insert(X(2), x2 * I_1x1);
QP qp = createTestCase(); QP qp = createTestCase();
DOUBLES_EQUAL(29, x1 * x1 - x1 * x2 + x2 * x2 - 3 * x1 + 5, 1e-9); DOUBLES_EQUAL(29, x1 * x1 - x1 * x2 + x2 * x2 - 3 * x1 + 5, 1e-9);
DOUBLES_EQUAL(29, qp.cost[0]->error(values), 1e-9); DOUBLES_EQUAL(29, qp.cost[0]->error(values), 1e-9);
@ -71,7 +71,7 @@ TEST(QPSolver, constraintsAux) {
VectorValues lambdas; VectorValues lambdas;
lambdas.insert(0, (Vector(1) << -0.5).finished()); lambdas.insert(0, (Vector(1) << -0.5).finished());
lambdas.insert(1, (Vector(1) << 0.0).finished()); lambdas.insert(1, kZero);
lambdas.insert(2, (Vector(1) << 0.3).finished()); lambdas.insert(2, (Vector(1) << 0.3).finished());
lambdas.insert(3, (Vector(1) << 0.1).finished()); lambdas.insert(3, (Vector(1) << 0.1).finished());
int factorIx = solver.identifyLeavingConstraint(qp.inequalities, lambdas); int factorIx = solver.identifyLeavingConstraint(qp.inequalities, lambdas);
@ -79,7 +79,7 @@ TEST(QPSolver, constraintsAux) {
VectorValues lambdas2; VectorValues lambdas2;
lambdas2.insert(0, (Vector(1) << -0.5).finished()); lambdas2.insert(0, (Vector(1) << -0.5).finished());
lambdas2.insert(1, (Vector(1) << 0.0).finished()); lambdas2.insert(1, kZero);
lambdas2.insert(2, (Vector(1) << -0.3).finished()); lambdas2.insert(2, (Vector(1) << -0.3).finished());
lambdas2.insert(3, (Vector(1) << -0.1).finished()); lambdas2.insert(3, (Vector(1) << -0.1).finished());
int factorIx2 = solver.identifyLeavingConstraint(qp.inequalities, lambdas2); int factorIx2 = solver.identifyLeavingConstraint(qp.inequalities, lambdas2);
@ -96,14 +96,14 @@ QP createEqualityConstrainedTest() {
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0 // Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0
qp.cost.push_back( qp.cost.push_back(
HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), Z_1x1, Z_1x1, HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1,
2.0 * Matrix::Ones(1, 1), Z_1x1, 0.0)); 2.0 * I_1x1, Z_1x1, 0.0));
// Equality constraints // Equality constraints
// x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector // x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector
Matrix A1 = (Matrix(1, 1) << 1).finished(); Matrix A1 = I_1x1;
Matrix A2 = (Matrix(1, 1) << 1).finished(); Matrix A2 = I_1x1;
Vector b = -(Vector(1) << 1).finished(); Vector b = -kOne;
qp.equalities.push_back(LinearEquality(X(1), A1, X(2), A2, b, 0)); qp.equalities.push_back(LinearEquality(X(1), A1, X(2), A2, b, 0));
return qp; return qp;
@ -119,7 +119,8 @@ TEST(QPSolver, dual) {
QPSolver solver(qp); QPSolver solver(qp);
GaussianFactorGraph::shared_ptr dualGraph = solver.buildDualGraph(qp.inequalities, initialValues); GaussianFactorGraph::shared_ptr dualGraph = solver.buildDualGraph(
qp.inequalities, initialValues);
VectorValues dual = dualGraph->optimize(); VectorValues dual = dualGraph->optimize();
VectorValues expectedDual; VectorValues expectedDual;
expectedDual.insert(0, (Vector(1) << 2.0).finished()); expectedDual.insert(0, (Vector(1) << 2.0).finished());
@ -135,18 +136,18 @@ TEST(QPSolver, indentifyActiveConstraints) {
currentSolution.insert(X(1), Z_1x1); currentSolution.insert(X(1), Z_1x1);
currentSolution.insert(X(2), Z_1x1); currentSolution.insert(X(2), Z_1x1);
InequalityFactorGraph workingSet = InequalityFactorGraph workingSet = solver.identifyActiveConstraints(
solver.identifyActiveConstraints(qp.inequalities, currentSolution); qp.inequalities, currentSolution);
CHECK(!workingSet.at(0)->active()); // inactive CHECK(!workingSet.at(0)->active()); // inactive
CHECK(workingSet.at(1)->active());// active CHECK(workingSet.at(1)->active()); // active
CHECK(workingSet.at(2)->active());// active CHECK(workingSet.at(2)->active()); // active
CHECK(!workingSet.at(3)->active());// inactive CHECK(!workingSet.at(3)->active()); // inactive
VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet); VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet);
VectorValues expectedSolution; VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 0.0).finished()); expectedSolution.insert(X(1), kZero);
expectedSolution.insert(X(2), (Vector(1) << 0.0).finished()); expectedSolution.insert(X(2), kZero);
CHECK(assert_equal(expectedSolution, solution, 1e-100)); CHECK(assert_equal(expectedSolution, solution, 1e-100));
} }
@ -161,13 +162,13 @@ TEST(QPSolver, iterate) {
currentSolution.insert(X(2), Z_1x1); currentSolution.insert(X(2), Z_1x1);
std::vector<VectorValues> expectedSolutions(4), expectedDuals(4); std::vector<VectorValues> expectedSolutions(4), expectedDuals(4);
expectedSolutions[0].insert(X(1), (Vector(1) << 0.0).finished()); expectedSolutions[0].insert(X(1), kZero);
expectedSolutions[0].insert(X(2), (Vector(1) << 0.0).finished()); expectedSolutions[0].insert(X(2), kZero);
expectedDuals[0].insert(1, (Vector(1) << 3).finished()); expectedDuals[0].insert(1, (Vector(1) << 3).finished());
expectedDuals[0].insert(2, (Vector(1) << 0).finished()); expectedDuals[0].insert(2, kZero);
expectedSolutions[1].insert(X(1), (Vector(1) << 1.5).finished()); expectedSolutions[1].insert(X(1), (Vector(1) << 1.5).finished());
expectedSolutions[1].insert(X(2), (Vector(1) << 0.0).finished()); expectedSolutions[1].insert(X(2), kZero);
expectedDuals[1].insert(3, (Vector(1) << 1.5).finished()); expectedDuals[1].insert(3, (Vector(1) << 1.5).finished());
expectedSolutions[2].insert(X(1), (Vector(1) << 1.5).finished()); expectedSolutions[2].insert(X(1), (Vector(1) << 1.5).finished());
@ -176,8 +177,8 @@ TEST(QPSolver, iterate) {
expectedSolutions[3].insert(X(1), (Vector(1) << 1.5).finished()); expectedSolutions[3].insert(X(1), (Vector(1) << 1.5).finished());
expectedSolutions[3].insert(X(2), (Vector(1) << 0.5).finished()); expectedSolutions[3].insert(X(2), (Vector(1) << 0.5).finished());
InequalityFactorGraph workingSet = InequalityFactorGraph workingSet = solver.identifyActiveConstraints(
solver.identifyActiveConstraints(qp.inequalities, currentSolution); qp.inequalities, currentSolution);
QPState state(currentSolution, VectorValues(), workingSet, false, 100); QPState state(currentSolution, VectorValues(), workingSet, false, 100);
@ -214,38 +215,46 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) {
pair<QP, QP> testParser(QPSParser parser) { pair<QP, QP> testParser(QPSParser parser) {
QP exampleqp = parser.Parse(); QP exampleqp = parser.Parse();
QP expectedqp; QP expectedqp;
Key X1(Symbol('X',1)), X2(Symbol('X',2)); Key X1(Symbol('X', 1)), X2(Symbol('X', 2));
// min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2 // min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2
expectedqp.cost.push_back( expectedqp.cost.push_back(
HessianFactor(X1, X2, 8.0 * ones(1, 1), 2.0 * ones(1, 1), HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, 1.5 * kOne,
1.5 * ones(1), 10.0 * ones(1, 1), -2.0 * ones(1), 4.0)); 10.0 * I_1x1, -2.0 * kOne, 4.0));
// 2x + y >= 2 // 2x + y >= 2
// -x + 2y <= 6 // -x + 2y <= 6
expectedqp.inequalities.push_back( expectedqp.inequalities.push_back(
LinearInequality(X2, -ones(1, 1), X1, -2.0 * ones(1, 1), -2, 0)); LinearInequality(X2, -I_1x1, X1, -2.0 * I_1x1, -2, 0));
expectedqp.inequalities.push_back( expectedqp.inequalities.push_back(
LinearInequality(X2, 2.0 * ones(1, 1), X1, -ones(1, 1), 6, 1)); LinearInequality(X2, 2.0 * I_1x1, X1, -I_1x1, 6, 1));
// x<= 20 // x<= 20
expectedqp.inequalities.push_back(LinearInequality(X1, ones(1, 1), 20, 4)); expectedqp.inequalities.push_back(LinearInequality(X1, I_1x1, 20, 4));
//x >= 0 //x >= 0
expectedqp.inequalities.push_back(LinearInequality(X1, -ones(1, 1), 0, 2)); expectedqp.inequalities.push_back(LinearInequality(X1, -I_1x1, 0, 2));
// y > = 0 // y > = 0
expectedqp.inequalities.push_back(LinearInequality(X2, -ones(1, 1), 0, 3)); expectedqp.inequalities.push_back(LinearInequality(X2, -I_1x1, 0, 3));
return std::make_pair(expectedqp, exampleqp); return std::make_pair(expectedqp, exampleqp);
}; }
;
TEST(QPSolver, ParserSyntaticTest){ TEST(QPSolver, ParserSyntaticTest) {
auto expectedActual = testParser(QPSParser("QPExample.QPS")); auto expectedActual = testParser(QPSParser("QPExample.QPS"));
CHECK(assert_equal(expectedActual.first.cost, expectedActual.second.cost, 1e-7)); CHECK(
CHECK(assert_equal(expectedActual.first.inequalities, expectedActual.second.inequalities, 1e-7)); assert_equal(expectedActual.first.cost, expectedActual.second.cost, 1e-7));
CHECK(assert_equal(expectedActual.first.equalities, expectedActual.second.equalities, 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) { 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(expected_actual.first).optimize(); boost::tie(expectedSolution, boost::tuples::ignore) = QPSolver(
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(expected_actual.second).optimize(); expected_actual.first).optimize();
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(
expected_actual.second).optimize();
CHECK(assert_equal(actualSolution, expectedSolution, 1e-7)); CHECK(assert_equal(actualSolution, expectedSolution, 1e-7));
} }
@ -259,15 +268,15 @@ QP createTestMatlabQPEx() {
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0 // Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0
qp.cost.push_back( qp.cost.push_back(
HessianFactor(X(1), X(2), 1.0 * I_1x1, -Matrix::Ones(1, 1), 2.0 * I_1x1, HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1,
2.0 * Matrix::Ones(1, 1), 6 * I_1x1, 1000.0)); 2.0 * I_1x1, 6 * I_1x1, 1000.0));
// Inequality constraints // Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), One, X(2), One, 2, 0)); // x1 + x2 <= 2 qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2
qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 1)); //-x1 + 2*x2 <=2 qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1)); //-x1 + 2*x2 <=2
qp.inequalities.push_back(LinearInequality(X(1), 2 * One, X(2), One, 3, 2)); // 2*x1 + x2 <=3 qp.inequalities.push_back(LinearInequality(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2)); // 2*x1 + x2 <=3
qp.inequalities.push_back(LinearInequality(X(1), -One, 0, 3)); // -x1 <= 0 qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 3)); // -x1 <= 0
qp.inequalities.push_back(LinearInequality(X(2), -One, 0, 4)); // -x2 <= 0 qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 4)); // -x2 <= 0
return qp; return qp;
} }
@ -304,18 +313,18 @@ TEST(QPSolver, optimizeMatlabExNoinitials) {
QP createTestNocedal06bookEx16_4() { QP createTestNocedal06bookEx16_4() {
QP qp; QP qp;
qp.cost.push_back(JacobianFactor(X(1), Matrix::Ones(1, 1), I_1x1)); qp.cost.push_back(JacobianFactor(X(1), I_1x1, I_1x1));
qp.cost.push_back(JacobianFactor(X(2), Matrix::Ones(1, 1), 2.5 * I_1x1)); qp.cost.push_back(JacobianFactor(X(2), I_1x1, 2.5 * I_1x1));
// Inequality constraints // Inequality constraints
qp.inequalities.push_back( qp.inequalities.push_back(
LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), 2, 0)); LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 0));
qp.inequalities.push_back( qp.inequalities.push_back(
LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6, 1)); LinearInequality(X(1), I_1x1, X(2), 2 * I_1x1, 6, 1));
qp.inequalities.push_back( qp.inequalities.push_back(
LinearInequality(X(1), ones(1, 1), X(2), -2 * ones(1, 1), 2, 2)); LinearInequality(X(1), I_1x1, X(2), -2 * I_1x1, 2, 2));
qp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), 0.0, 3)); qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0.0, 3));
qp.inequalities.push_back(LinearInequality(X(2), -ones(1, 1), 0.0, 4)); qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0.0, 4));
return qp; return qp;
} }
@ -341,7 +350,7 @@ TEST(QPSolver, failedSubproblem) {
qp.cost.push_back(JacobianFactor(X(1), I_2x2, Z_2x1)); qp.cost.push_back(JacobianFactor(X(1), I_2x2, Z_2x1));
qp.cost.push_back(HessianFactor(X(1), Z_2x2, Z_2x1, 100.0)); qp.cost.push_back(HessianFactor(X(1), Z_2x2, Z_2x1, 100.0));
qp.inequalities.push_back( qp.inequalities.push_back(
LinearInequality(X(1), (Matrix(1,2) << -1.0, 0.0).finished(), -1.0, 0)); LinearInequality(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0));
VectorValues expected; VectorValues expected;
expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished()); expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished());
@ -359,10 +368,10 @@ TEST(QPSolver, failedSubproblem) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(QPSolver, infeasibleInitial) { TEST(QPSolver, infeasibleInitial) {
QP qp; QP qp;
qp.cost.push_back(JacobianFactor(X(1), eye(2), zero(2))); qp.cost.push_back(JacobianFactor(X(1), I_2x2, Vector::Zero(2)));
qp.cost.push_back(HessianFactor(X(1), zeros(2, 2), zero(2), 100.0)); qp.cost.push_back(HessianFactor(X(1), Z_2x2, Vector::Zero(2), 100.0));
qp.inequalities.push_back( qp.inequalities.push_back(
LinearInequality(X(1), (Matrix(1,2) << -1.0, 0.0).finished(), -1.0, 0)); LinearInequality(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0));
VectorValues expected; VectorValues expected;
expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished()); expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished());
@ -372,10 +381,7 @@ TEST(QPSolver, infeasibleInitial) {
QPSolver solver(qp); QPSolver solver(qp);
VectorValues solution; VectorValues solution;
CHECK_EXCEPTION( CHECK_EXCEPTION(solver.optimize(initialValues), InfeasibleInitialValues);
solver.optimize(initialValues),
InfeasibleInitialValues
);
} }
/* ************************************************************************* */ /* ************************************************************************* */