indent
Duy-Nguyen Ta
parent
6d04d1e944
commit
6d9ad2d4eb
|
|
@ -47,12 +47,17 @@ static const Vector kOne = Vector::Ones(1), kZero = Vector::Zero(1);
|
|||
*/
|
||||
LP simpleLP1() {
|
||||
LP lp;
|
||||
lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2)
|
||||
lp.inequalities.push_back(LinearInequality(1, Vector2(-1, 0), 0, 1)); // x1 >= 0
|
||||
lp.inequalities.push_back(LinearInequality(1, Vector2(0, -1), 0, 2)); // x2 >= 0
|
||||
lp.inequalities.push_back(LinearInequality(1, Vector2(1, 2), 4, 3)); // x1 + 2*x2 <= 4
|
||||
lp.inequalities.push_back(LinearInequality(1, Vector2(4, 2), 12, 4)); // 4x1 + 2x2 <= 12
|
||||
lp.inequalities.push_back(LinearInequality(1, Vector2(-1, 1), 1, 5)); // -x1 + x2 <= 1
|
||||
lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2)
|
||||
lp.inequalities.push_back(
|
||||
LinearInequality(1, Vector2(-1, 0), 0, 1)); // x1 >= 0
|
||||
lp.inequalities.push_back(
|
||||
LinearInequality(1, Vector2(0, -1), 0, 2)); // x2 >= 0
|
||||
lp.inequalities.push_back(
|
||||
LinearInequality(1, Vector2(1, 2), 4, 3)); // x1 + 2*x2 <= 4
|
||||
lp.inequalities.push_back(
|
||||
LinearInequality(1, Vector2(4, 2), 12, 4)); // 4x1 + 2x2 <= 12
|
||||
lp.inequalities.push_back(
|
||||
LinearInequality(1, Vector2(-1, 1), 1, 5)); // -x1 + x2 <= 1
|
||||
return lp;
|
||||
}
|
||||
|
||||
|
|
@ -61,15 +66,20 @@ namespace gtsam {
|
|||
|
||||
TEST(LPInitSolver, infinite_loop_single_var) {
|
||||
LP initchecker;
|
||||
initchecker.cost = LinearCost(1,Vector3(0,0,1)); //min alpha
|
||||
initchecker.inequalities.push_back(LinearInequality(1, Vector3(-2,-1,-1),-2,1));//-2x-y-alpha <= -2
|
||||
initchecker.inequalities.push_back(LinearInequality(1, Vector3(-1,2,-1), 6, 2));// -x+2y-alpha <= 6
|
||||
initchecker.inequalities.push_back(LinearInequality(1, Vector3(-1,0,-1), 0,3));// -x - alpha <= 0
|
||||
initchecker.inequalities.push_back(LinearInequality(1, Vector3(1,0,-1), 20, 4));//x - alpha <= 20
|
||||
initchecker.inequalities.push_back(LinearInequality(1, Vector3(0,-1,-1),0, 5));// -y - alpha <= 0
|
||||
initchecker.cost = LinearCost(1, Vector3(0, 0, 1)); // min alpha
|
||||
initchecker.inequalities.push_back(
|
||||
LinearInequality(1, Vector3(-2, -1, -1), -2, 1)); //-2x-y-alpha <= -2
|
||||
initchecker.inequalities.push_back(
|
||||
LinearInequality(1, Vector3(-1, 2, -1), 6, 2)); // -x+2y-alpha <= 6
|
||||
initchecker.inequalities.push_back(
|
||||
LinearInequality(1, Vector3(-1, 0, -1), 0, 3)); // -x - alpha <= 0
|
||||
initchecker.inequalities.push_back(
|
||||
LinearInequality(1, Vector3(1, 0, -1), 20, 4)); // x - alpha <= 20
|
||||
initchecker.inequalities.push_back(
|
||||
LinearInequality(1, Vector3(0, -1, -1), 0, 5)); // -y - alpha <= 0
|
||||
LPSolver solver(initchecker);
|
||||
VectorValues starter;
|
||||
starter.insert(1,Vector3(0,0,2));
|
||||
starter.insert(1, Vector3(0, 0, 2));
|
||||
VectorValues results, duals;
|
||||
boost::tie(results, duals) = solver.optimize(starter);
|
||||
VectorValues expected;
|
||||
|
|
@ -82,14 +92,19 @@ TEST(LPInitSolver, infinite_loop_multi_var) {
|
|||
Key X = symbol('X', 1);
|
||||
Key Y = symbol('Y', 1);
|
||||
Key Z = symbol('Z', 1);
|
||||
initchecker.cost = LinearCost(Z, kOne); //min alpha
|
||||
initchecker.cost = LinearCost(Z, kOne); // min alpha
|
||||
initchecker.inequalities.push_back(
|
||||
LinearInequality(X, -2.0 * kOne, Y, -1.0 * kOne, Z, -1.0 * kOne, -2, 1));//-2x-y-alpha <= -2
|
||||
LinearInequality(X, -2.0 * kOne, Y, -1.0 * kOne, Z, -1.0 * kOne, -2,
|
||||
1)); //-2x-y-alpha <= -2
|
||||
initchecker.inequalities.push_back(
|
||||
LinearInequality(X, -1.0 * kOne, Y, 2.0 * kOne, Z, -1.0 * kOne, 6, 2));// -x+2y-alpha <= 6
|
||||
initchecker.inequalities.push_back(LinearInequality(X, -1.0 * kOne, Z, -1.0 * kOne, 0, 3));// -x - alpha <= 0
|
||||
initchecker.inequalities.push_back(LinearInequality(X, 1.0 * kOne, Z, -1.0 * kOne, 20, 4));//x - alpha <= 20
|
||||
initchecker.inequalities.push_back(LinearInequality(Y, -1.0 * kOne, Z, -1.0 * kOne, 0, 5));// -y - alpha <= 0
|
||||
LinearInequality(X, -1.0 * kOne, Y, 2.0 * kOne, Z, -1.0 * kOne, 6,
|
||||
2)); // -x+2y-alpha <= 6
|
||||
initchecker.inequalities.push_back(LinearInequality(
|
||||
X, -1.0 * kOne, Z, -1.0 * kOne, 0, 3)); // -x - alpha <= 0
|
||||
initchecker.inequalities.push_back(LinearInequality(
|
||||
X, 1.0 * kOne, Z, -1.0 * kOne, 20, 4)); // x - alpha <= 20
|
||||
initchecker.inequalities.push_back(LinearInequality(
|
||||
Y, -1.0 * kOne, Z, -1.0 * kOne, 0, 5)); // -y - alpha <= 0
|
||||
LPSolver solver(initchecker);
|
||||
VectorValues starter;
|
||||
starter.insert(X, kZero);
|
||||
|
|
@ -108,7 +123,8 @@ TEST(LPInitSolver, initialization) {
|
|||
LP lp = simpleLP1();
|
||||
LPInitSolver initSolver(lp);
|
||||
|
||||
GaussianFactorGraph::shared_ptr initOfInitGraph = initSolver.buildInitOfInitGraph();
|
||||
GaussianFactorGraph::shared_ptr initOfInitGraph =
|
||||
initSolver.buildInitOfInitGraph();
|
||||
VectorValues x0 = initOfInitGraph->optimize();
|
||||
VectorValues expected_x0;
|
||||
expected_x0.insert(1, Vector::Zero(2));
|
||||
|
|
@ -122,16 +138,19 @@ TEST(LPInitSolver, initialization) {
|
|||
LP::shared_ptr initLP = initSolver.buildInitialLP(yKey);
|
||||
LP expectedInitLP;
|
||||
expectedInitLP.cost = LinearCost(yKey, kOne);
|
||||
expectedInitLP.inequalities.push_back(LinearInequality(
|
||||
1, Vector2(-1, 0), 2, Vector::Constant(1, -1), 0, 1)); // -x1 - y <= 0
|
||||
expectedInitLP.inequalities.push_back(LinearInequality(
|
||||
1, Vector2(0, -1), 2, Vector::Constant(1, -1), 0, 2)); // -x2 - y <= 0
|
||||
expectedInitLP.inequalities.push_back(
|
||||
LinearInequality(1, Vector2( -1, 0), 2, Vector::Constant(1, -1), 0, 1)); // -x1 - y <= 0
|
||||
LinearInequality(1, Vector2(1, 2), 2, Vector::Constant(1, -1), 4,
|
||||
3)); // x1 + 2*x2 - y <= 4
|
||||
expectedInitLP.inequalities.push_back(
|
||||
LinearInequality(1, Vector2( 0, -1), 2, Vector::Constant(1, -1), 0, 2));// -x2 - y <= 0
|
||||
LinearInequality(1, Vector2(4, 2), 2, Vector::Constant(1, -1), 12,
|
||||
4)); // 4x1 + 2x2 - y <= 12
|
||||
expectedInitLP.inequalities.push_back(
|
||||
LinearInequality(1, Vector2( 1, 2), 2, Vector::Constant(1, -1), 4, 3));// x1 + 2*x2 - y <= 4
|
||||
expectedInitLP.inequalities.push_back(
|
||||
LinearInequality(1, Vector2( 4, 2), 2, Vector::Constant(1, -1), 12, 4));// 4x1 + 2x2 - y <= 12
|
||||
expectedInitLP.inequalities.push_back(
|
||||
LinearInequality(1, Vector2( -1, 1), 2, Vector::Constant(1, -1), 1, 5));// -x1 + x2 - y <= 1
|
||||
LinearInequality(1, Vector2(-1, 1), 2, Vector::Constant(1, -1), 1,
|
||||
5)); // -x1 + x2 - y <= 1
|
||||
CHECK(assert_equal(expectedInitLP, *initLP, 1e-10));
|
||||
LPSolver lpSolveInit(*initLP);
|
||||
VectorValues xy0(x0);
|
||||
|
|
@ -155,56 +174,61 @@ TEST(LPInitSolver, initialization) {
|
|||
* x + 2y = 6
|
||||
*/
|
||||
TEST(LPSolver, overConstrainedLinearSystem) {
|
||||
GaussianFactorGraph graph;
|
||||
Matrix A1 = Vector3(1,1,1);
|
||||
Matrix A2 = Vector3(1,-1,2);
|
||||
Vector b = Vector3( 1, 5, 6);
|
||||
JacobianFactor factor(1, A1, 2, A2, b, noiseModel::Constrained::All(3));
|
||||
graph.push_back(factor);
|
||||
GaussianFactorGraph graph;
|
||||
Matrix A1 = Vector3(1, 1, 1);
|
||||
Matrix A2 = Vector3(1, -1, 2);
|
||||
Vector b = Vector3(1, 5, 6);
|
||||
JacobianFactor factor(1, A1, 2, A2, b, noiseModel::Constrained::All(3));
|
||||
graph.push_back(factor);
|
||||
|
||||
VectorValues x = graph.optimize();
|
||||
// This check confirms that gtsam linear constraint solver can't handle over-constrained system
|
||||
CHECK(factor.error(x) != 0.0);
|
||||
VectorValues x = graph.optimize();
|
||||
// This check confirms that gtsam linear constraint solver can't handle
|
||||
// over-constrained system
|
||||
CHECK(factor.error(x) != 0.0);
|
||||
}
|
||||
|
||||
TEST(LPSolver, overConstrainedLinearSystem2) {
|
||||
GaussianFactorGraph graph;
|
||||
graph.push_back(JacobianFactor(1, I_1x1, 2, I_1x1, kOne, noiseModel::Constrained::All(1)));
|
||||
graph.push_back(JacobianFactor(1, I_1x1, 2, -I_1x1, 5*kOne, noiseModel::Constrained::All(1)));
|
||||
graph.push_back(JacobianFactor(1, I_1x1, 2, 2*I_1x1, 6*kOne, noiseModel::Constrained::All(1)));
|
||||
VectorValues x = graph.optimize();
|
||||
// This check confirms that gtsam linear constraint solver can't handle over-constrained system
|
||||
CHECK(graph.error(x) != 0.0);
|
||||
GaussianFactorGraph graph;
|
||||
graph.push_back(JacobianFactor(1, I_1x1, 2, I_1x1, kOne,
|
||||
noiseModel::Constrained::All(1)));
|
||||
graph.push_back(JacobianFactor(1, I_1x1, 2, -I_1x1, 5 * kOne,
|
||||
noiseModel::Constrained::All(1)));
|
||||
graph.push_back(JacobianFactor(1, I_1x1, 2, 2 * I_1x1, 6 * kOne,
|
||||
noiseModel::Constrained::All(1)));
|
||||
VectorValues x = graph.optimize();
|
||||
// This check confirms that gtsam linear constraint solver can't handle
|
||||
// over-constrained system
|
||||
CHECK(graph.error(x) != 0.0);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(LPSolver, simpleTest1) {
|
||||
LP lp = simpleLP1();
|
||||
LPSolver lpSolver(lp);
|
||||
VectorValues init;
|
||||
init.insert(1, Vector::Zero(2));
|
||||
LP lp = simpleLP1();
|
||||
LPSolver lpSolver(lp);
|
||||
VectorValues init;
|
||||
init.insert(1, Vector::Zero(2));
|
||||
|
||||
VectorValues expected_x1;
|
||||
expected_x1.insert(1, Vector::Ones(2));
|
||||
CHECK(assert_equal(expected_x1, x1, 1e-10));
|
||||
|
||||
VectorValues result, duals;
|
||||
boost::tie(result, duals) = lpSolver.optimize(init);
|
||||
VectorValues expectedResult;
|
||||
expectedResult.insert(1, Vector2(8./3., 2./3.));
|
||||
CHECK(assert_equal(expectedResult, result, 1e-10));
|
||||
VectorValues x1 =
|
||||
lpSolver.buildWorkingGraph(InequalityFactorGraph(), init).optimize();
|
||||
VectorValues expected_x1;
|
||||
expected_x1.insert(1, Vector::Ones(2));
|
||||
CHECK(assert_equal(expected_x1, x1, 1e-10));
|
||||
|
||||
VectorValues result, duals;
|
||||
boost::tie(result, duals) = lpSolver.optimize(init);
|
||||
VectorValues expectedResult;
|
||||
expectedResult.insert(1, Vector2(8. / 3., 2. / 3.));
|
||||
CHECK(assert_equal(expectedResult, result, 1e-10));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(LPSolver, testWithoutInitialValues) {
|
||||
LP lp = simpleLP1();
|
||||
LPSolver lpSolver(lp);
|
||||
VectorValues result,duals, expectedResult;
|
||||
expectedResult.insert(1, Vector2(8./3., 2./3.));
|
||||
boost::tie(result, duals) = lpSolver.optimize();
|
||||
CHECK(assert_equal(expectedResult, result));
|
||||
LP lp = simpleLP1();
|
||||
LPSolver lpSolver(lp);
|
||||
VectorValues result, duals, expectedResult;
|
||||
expectedResult.insert(1, Vector2(8. / 3., 2. / 3.));
|
||||
boost::tie(result, duals) = lpSolver.optimize();
|
||||
CHECK(assert_equal(expectedResult, result));
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -215,18 +239,17 @@ CHECK(assert_equal(expectedResult, result));
|
|||
*/
|
||||
/* ************************************************************************* */
|
||||
TEST(LPSolver, LinearCost) {
|
||||
LinearCost cost(1, Vector3( 2., 4., 6.));
|
||||
VectorValues x;
|
||||
x.insert(1, Vector3( 1., 3., 5.));
|
||||
double error = cost.error(x);
|
||||
double expectedError = 44.0;
|
||||
DOUBLES_EQUAL(expectedError, error, 1e-100);
|
||||
LinearCost cost(1, Vector3(2., 4., 6.));
|
||||
VectorValues x;
|
||||
x.insert(1, Vector3(1., 3., 5.));
|
||||
double error = cost.error(x);
|
||||
double expectedError = 44.0;
|
||||
DOUBLES_EQUAL(expectedError, error, 1e-100);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue