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

2
gtsam_unstable/linear/QPSolver.h
View File

@ -150,7 +150,7 @@ public:
* @return a pair of <primal, dual> solutions * @return a pair of <primal, dual> solutions
*/ */
std::pair<VectorValues, VectorValues> optimize( std::pair<VectorValues, VectorValues> optimize(
const VectorValues& initials) const; const VectorValues& initialValues) const;
/** Optimize without an initial value. /** Optimize without an initial value.
* This version of optimize will try to find a feasible initial value by solving * 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(); GaussianFactorGraph graph = createEqualityConstrainedTest();
// Initials values // Initials values
VectorValues initials; VectorValues initialValues;
initials.insert(X(1), ones(1)); initialValues.insert(X(1), ones(1));
initials.insert(X(2), ones(1)); initialValues.insert(X(2), ones(1));
QPSolver solver(graph); QPSolver solver(graph);
GaussianFactorGraph dualGraph = solver.buildDualGraph(graph, initials); GaussianFactorGraph dualGraph = solver.buildDualGraph(graph, initialValues);
VectorValues dual = dualGraph.optimize(); VectorValues dual = dualGraph.optimize();
VectorValues expectedDual; VectorValues expectedDual;
expectedDual.insert(1, (Vector(1) << 2.0)); expectedDual.insert(1, (Vector(1) << 2.0));
@ -172,11 +172,11 @@ TEST(QPSolver, iterate) {
TEST(QPSolver, optimizeForst10book_pg171Ex5) { TEST(QPSolver, optimizeForst10book_pg171Ex5) {
GaussianFactorGraph graph = createTestCase(); GaussianFactorGraph graph = createTestCase();
QPSolver solver(graph); QPSolver solver(graph);
VectorValues initials; VectorValues initialValues;
initials.insert(X(1), zero(1)); initialValues.insert(X(1), zero(1));
initials.insert(X(2), zero(1)); initialValues.insert(X(2), zero(1));
VectorValues solution; VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initials); boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
VectorValues expectedSolution; VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 1.5)); expectedSolution.insert(X(1), (Vector(1) << 1.5));
expectedSolution.insert(X(2), (Vector(1) << 0.5)); expectedSolution.insert(X(2), (Vector(1) << 0.5));
@ -213,11 +213,11 @@ GaussianFactorGraph createTestMatlabQPEx() {
TEST(QPSolver, optimizeMatlabEx) { TEST(QPSolver, optimizeMatlabEx) {
GaussianFactorGraph graph = createTestMatlabQPEx(); GaussianFactorGraph graph = createTestMatlabQPEx();
QPSolver solver(graph); QPSolver solver(graph);
VectorValues initials; VectorValues initialValues;
initials.insert(X(1), zero(1)); initialValues.insert(X(1), zero(1));
initials.insert(X(2), zero(1)); initialValues.insert(X(2), zero(1));
VectorValues solution; VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initials); boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
VectorValues expectedSolution; VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0)); expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0));
expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0)); expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0));
@ -253,12 +253,12 @@ GaussianFactorGraph createTestNocedal06bookEx16_4() {
TEST(QPSolver, optimizeNocedal06bookEx16_4) { TEST(QPSolver, optimizeNocedal06bookEx16_4) {
GaussianFactorGraph graph = createTestNocedal06bookEx16_4(); GaussianFactorGraph graph = createTestNocedal06bookEx16_4();
QPSolver solver(graph); QPSolver solver(graph);
VectorValues initials; VectorValues initialValues;
initials.insert(X(1), (Vector(1) << 2.0)); initialValues.insert(X(1), (Vector(1) << 2.0));
initials.insert(X(2), zero(1)); initialValues.insert(X(2), zero(1));
VectorValues solution; VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initials); boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
VectorValues expectedSolution; VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 1.4)); expectedSolution.insert(X(1), (Vector(1) << 1.4));
expectedSolution.insert(X(2), (Vector(1) << 1.7)); expectedSolution.insert(X(2), (Vector(1) << 1.7));
@ -356,10 +356,10 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4_findInitialPoint) {
EXPECT(assert_equal(expectedConstraints, *constraints)); EXPECT(assert_equal(expectedConstraints, *constraints));
bool isFeasible; bool isFeasible;
VectorValues initials; VectorValues initialValues;
boost::tie(isFeasible, initials) = solver.findFeasibleInitialValues(); boost::tie(isFeasible, initialValues) = solver.findFeasibleInitialValues();
EXPECT(assert_equal(1.0*ones(1), initials.at(X(1)))); EXPECT(assert_equal(1.0*ones(1), initialValues.at(X(1))));
EXPECT(assert_equal(0.0*ones(1), initials.at(X(2)))); EXPECT(assert_equal(0.0*ones(1), initialValues.at(X(2))));
VectorValues solution; VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(); boost::tie(solution, boost::tuples::ignore) = solver.optimize();
@ -370,16 +370,16 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4_findInitialPoint) {
TEST(QPSolver, optimizeNocedal06bookEx16_4_2) { TEST(QPSolver, optimizeNocedal06bookEx16_4_2) {
GaussianFactorGraph graph = createTestNocedal06bookEx16_4(); GaussianFactorGraph graph = createTestNocedal06bookEx16_4();
QPSolver solver(graph); QPSolver solver(graph);
VectorValues initials; VectorValues initialValues;
initials.insert(X(1), (Vector(1) << 0.0)); initialValues.insert(X(1), (Vector(1) << 0.0));
initials.insert(X(2), (Vector(1) << 100.0)); initialValues.insert(X(2), (Vector(1) << 100.0));
VectorValues expectedSolution; VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 1.4)); expectedSolution.insert(X(1), (Vector(1) << 1.4));
expectedSolution.insert(X(2), (Vector(1) << 1.7)); expectedSolution.insert(X(2), (Vector(1) << 1.7));
VectorValues solution; 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!! // THIS should fail because of the bad infeasible initial point!!
// CHECK(assert_equal(expectedSolution, solution, 1e-7)); // CHECK(assert_equal(expectedSolution, solution, 1e-7));