29 using namespace gtsam;
46 2.0 * I_1x1, Z_1x1, 10.0));
60 double x1 = 5,
x2 = 7;
100 2.0 * I_1x1, Z_1x1, 0.0));
117 initialValues.
insert(
X(1), I_1x1);
118 initialValues.
insert(
X(2), I_1x1);
135 currentSolution.
insert(
X(1), Z_1x1);
136 currentSolution.
insert(
X(2), Z_1x1);
141 CHECK(!workingSet.at(0)->active());
142 CHECK(workingSet.at(1)->active());
143 CHECK(workingSet.at(2)->active());
144 CHECK(!workingSet.at(3)->active());
159 currentSolution.
insert(
X(1), Z_1x1);
160 currentSolution.
insert(
X(2), Z_1x1);
162 std::vector<VectorValues>
expected(4), expectedDuals(4);
165 expectedDuals[0].insert(1, (
Vector(1) << 3).finished());
166 expectedDuals[0].insert(2,
kZero);
170 expectedDuals[1].insert(3, (
Vector(1) << 1.5).finished());
185 while (!
state.converged) {
205 initialValues.
insert(
X(1), Z_1x1);
206 initialValues.
insert(
X(2), Z_1x1);
220 -1.5 *
kOne, 10.0 * I_1x1, 2.0 *
kOne,
223 expected.inequalities.add(
X1, -2.0 * I_1x1,
X2, -I_1x1, -2, 0);
224 expected.inequalities.add(
X1, -I_1x1,
X2, 2.0 * I_1x1, 6, 1);
254 double error_actual = problem.
cost.
error(actual);
265 double error_actual = problem.
cost.
error(actual);
273 double error_actual = problem.
cost.
error(actual);
280 double error_actual = problem.
cost.
error(actual);
287 double error_actual = problem.
cost.
error(actual);
294 double error_actual = problem.
cost.
error(actual);
302 double error_actual = problem.
cost.
error(actual);
309 double error_actual = problem.
cost.
error(actual);
325 2.0 * I_1x1, 6 * I_1x1, 1000.0));
343 initialValues.
insert(
X(1), Z_1x1);
344 initialValues.
insert(
X(2), Z_1x1);
370 qp.
cost.
add(
X(2), I_1x1, 2.5 * I_1x1);
387 initialValues.
insert(
X(2), Z_1x1);
407 initialValues.
insert(
X(1), (
Vector(2) << 10.0, 100.0).finished());
418 qp.
cost.
add(
X(1), I_2x2, Vector::Zero(2));
426 initialValues.
insert(
X(1), (
Vector(2) << -10.0, 100.0).finished());