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

initials -> initialValues

release/4.3a0
dellaert 2014-11-27 10:52:25 +01:00
parent 4871202664
commit 3800e1f101
3 changed files with 38 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam_unstable/linear/QPSolver.cpp
View File

@ -412,9 +412,9 @@ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph,
//******************************************************************************
pair<VectorValues, VectorValues> QPSolver::optimize(
const VectorValues& initials) const {
const VectorValues& initialValues) const {
GaussianFactorGraph workingGraph = graph_.clone();
VectorValues currentSolution = initials;
VectorValues currentSolution = initialValues;
VectorValues lambdas;
bool converged = false;
while (!converged) {
@ -432,15 +432,15 @@ pair<VectorValues, Key> QPSolver::initialValuesLP() const {
}
firstSlackKey += 1;
VectorValues initials;
VectorValues initialValues;
// Create zero values for constrained vars
BOOST_FOREACH(size_t iFactor, constraintIndices_) {
JacobianFactor::shared_ptr jacobian = toJacobian(graph_.at(iFactor));
KeyVector keys = jacobian->keys();
BOOST_FOREACH(Key key, keys) {
if (!initials.exists(key)) {
if (!initialValues.exists(key)) {
size_t dim = jacobian->getDim(jacobian->find(key));
initials.insert(key, zero(dim));
initialValues.insert(key, zero(dim));
}
}
}
@ -459,10 +459,10 @@ pair<VectorValues, Key> QPSolver::initialValuesLP() const {
errorAtZero[i] = fabs(errorAtZero[i]);
} // if it has >0 sigma, i.e. normal Gaussian noise, initialize it at 0
}
initials.insert(slackKey, slackInit);
initialValues.insert(slackKey, slackInit);
slackKey++;
}
return make_pair(initials, firstSlackKey);
return make_pair(initialValues, firstSlackKey);
}
//******************************************************************************
@ -518,9 +518,9 @@ pair<GaussianFactorGraph::shared_ptr, VectorValues> QPSolver::constraintsLP(
pair<bool, VectorValues> QPSolver::findFeasibleInitialValues() const {
static const bool debug = false;
// Initial values with slack variables for the LP subproblem, Nocedal06book, pg.473
VectorValues initials;
VectorValues initialValues;
size_t firstSlackKey;
boost::tie(initials, firstSlackKey) = initialValuesLP();
boost::tie(initialValues, firstSlackKey) = initialValuesLP();
// Coefficients for the LP subproblem objective function, min \sum_i z_i
VectorValues objectiveLP = objectiveCoeffsLP(firstSlackKey);
@ -535,7 +535,7 @@ pair<bool, VectorValues> QPSolver::findFeasibleInitialValues() const {
VectorValues solution = lpSolver.solve();
if (debug)
initials.print("Initials LP: ");
initialValues.print("Initials LP: ");
if (debug)
objectiveLP.print("Objective LP: ");
if (debug)
@ -567,12 +567,12 @@ pair<bool, VectorValues> QPSolver::findFeasibleInitialValues() const {
//******************************************************************************
pair<VectorValues, VectorValues> QPSolver::optimize() const {
bool isFeasible;
VectorValues initials;
boost::tie(isFeasible, initials) = findFeasibleInitialValues();
VectorValues initialValues;
boost::tie(isFeasible, initialValues) = findFeasibleInitialValues();
if (!isFeasible) {
throw runtime_error("LP subproblem is infeasible!");
}
return optimize(initials);
return optimize(initialValues);
}
} /* namespace gtsam */

2
gtsam_unstable/linear/QPSolver.h
View File

@ -150,7 +150,7 @@ public:
* @return a pair of <primal, dual> solutions
*/
std::pair<VectorValues, VectorValues> optimize(
const VectorValues& initials) const;
const VectorValues& initialValues) const;
/** Optimize without an initial value.
* This version of optimize will try to find a feasible initial value by solving

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

@ -124,13 +124,13 @@ TEST(QPSolver, dual) {
GaussianFactorGraph graph = createEqualityConstrainedTest();
// Initials values
VectorValues initials;
initials.insert(X(1), ones(1));
initials.insert(X(2), ones(1));
VectorValues initialValues;
initialValues.insert(X(1), ones(1));
initialValues.insert(X(2), ones(1));
QPSolver solver(graph);
GaussianFactorGraph dualGraph = solver.buildDualGraph(graph, initials);
GaussianFactorGraph dualGraph = solver.buildDualGraph(graph, initialValues);
VectorValues dual = dualGraph.optimize();
VectorValues expectedDual;
expectedDual.insert(1, (Vector(1) << 2.0));
@ -172,11 +172,11 @@ TEST(QPSolver, iterate) {
TEST(QPSolver, optimizeForst10book_pg171Ex5) {
GaussianFactorGraph graph = createTestCase();
QPSolver solver(graph);
VectorValues initials;
initials.insert(X(1), zero(1));
initials.insert(X(2), zero(1));
VectorValues initialValues;
initialValues.insert(X(1), zero(1));
initialValues.insert(X(2), zero(1));
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initials);
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 1.5));
expectedSolution.insert(X(2), (Vector(1) << 0.5));
@ -213,11 +213,11 @@ GaussianFactorGraph createTestMatlabQPEx() {
TEST(QPSolver, optimizeMatlabEx) {
GaussianFactorGraph graph = createTestMatlabQPEx();
QPSolver solver(graph);
VectorValues initials;
initials.insert(X(1), zero(1));
initials.insert(X(2), zero(1));
VectorValues initialValues;
initialValues.insert(X(1), zero(1));
initialValues.insert(X(2), zero(1));
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initials);
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0));
expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0));
@ -253,12 +253,12 @@ GaussianFactorGraph createTestNocedal06bookEx16_4() {
TEST(QPSolver, optimizeNocedal06bookEx16_4) {
GaussianFactorGraph graph = createTestNocedal06bookEx16_4();
QPSolver solver(graph);
VectorValues initials;
initials.insert(X(1), (Vector(1) << 2.0));
initials.insert(X(2), zero(1));
VectorValues initialValues;
initialValues.insert(X(1), (Vector(1) << 2.0));
initialValues.insert(X(2), zero(1));
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initials);
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 1.4));
expectedSolution.insert(X(2), (Vector(1) << 1.7));
@ -356,10 +356,10 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4_findInitialPoint) {
EXPECT(assert_equal(expectedConstraints, *constraints));
bool isFeasible;
VectorValues initials;
boost::tie(isFeasible, initials) = solver.findFeasibleInitialValues();
EXPECT(assert_equal(1.0*ones(1), initials.at(X(1))));
EXPECT(assert_equal(0.0*ones(1), initials.at(X(2))));
VectorValues initialValues;
boost::tie(isFeasible, initialValues) = solver.findFeasibleInitialValues();
EXPECT(assert_equal(1.0*ones(1), initialValues.at(X(1))));
EXPECT(assert_equal(0.0*ones(1), initialValues.at(X(2))));
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize();
@ -370,16 +370,16 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4_findInitialPoint) {
TEST(QPSolver, optimizeNocedal06bookEx16_4_2) {
GaussianFactorGraph graph = createTestNocedal06bookEx16_4();
QPSolver solver(graph);
VectorValues initials;
initials.insert(X(1), (Vector(1) << 0.0));
initials.insert(X(2), (Vector(1) << 100.0));
VectorValues initialValues;
initialValues.insert(X(1), (Vector(1) << 0.0));
initialValues.insert(X(2), (Vector(1) << 100.0));
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 1.4));
expectedSolution.insert(X(2), (Vector(1) << 1.7));
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initials);
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
// THIS should fail because of the bad infeasible initial point!!
// CHECK(assert_equal(expectedSolution, solution, 1e-7));