From 9ca2ba9b660ddbe55b40e960326928aa27502bfc Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 27 Nov 2014 10:47:45 +0100 Subject: [PATCH 01/13] Simplified freeHessians_ using inner class --- .cproject | 2148 +++++++++--------- gtsam_unstable/linear/QPSolver.cpp | 82 +- gtsam_unstable/linear/QPSolver.h | 32 +- gtsam_unstable/linear/tests/testQPSolver.cpp | 10 +- 4 files changed, 1082 insertions(+), 1190 deletions(-) diff --git a/.cproject b/.cproject index 30c3d3327..7ae530863 100644 --- a/.cproject +++ b/.cproject @@ -1,17 +1,19 @@ - + + + + + - - @@ -60,13 +62,13 @@ + + - - @@ -116,13 +118,13 @@ + + - - @@ -582,7 +584,6 @@ make - tests/testBayesTree.run true false @@ -590,7 +591,6 @@ make - testBinaryBayesNet.run true false @@ -638,7 +638,6 @@ make - testSymbolicBayesNet.run true false @@ -646,7 +645,6 @@ make - tests/testSymbolicFactor.run true false @@ -654,7 +652,6 @@ make - testSymbolicFactorGraph.run true false @@ -670,20 +667,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -740,46 +728,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - all - true - false - true - - - make - -j5 - check - true - false - true - make -j2 @@ -788,31 +736,7 @@ true true - - make - -j5 - testGaussianFactorGraphUnordered.run - true - true - true - - - make - -j5 - testGaussianBayesNetUnordered.run - true - true - true - - - make - -j2 - clean - true - true - true - - + make -j2 all @@ -820,7 +744,15 @@ true true - + + make + -j2 + check + true + true + true + + make -j2 clean @@ -828,78 +760,6 @@ true true - - make - -j2 - clean all - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j6 -j8 - testAntiFactor.run - true - true - true - - - make - -j6 -j8 - testBetweenFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - make -j5 @@ -940,30 +800,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - make -j5 @@ -1006,40 +842,23 @@ make - testErrors.run true true true - + make - -j2 - check + -j5 + testGaussianFactorGraphUnordered.run true true true - + make - -j2 - timeCalibratedCamera.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - clean + -j5 + testGaussianBayesNetUnordered.run true true true @@ -1108,379 +927,6 @@ true true - - make - -j5 - testDiscreteFactor.run - true - true - true - - - make - -j1 - testDiscreteBayesTree.run - true - false - true - - - make - -j5 - testDiscreteFactorGraph.run - true - true - true - - - make - -j5 - testDiscreteConditional.run - true - true - true - - - make - -j5 - testDiscreteMarginals.run - true - true - true - - - make - -j5 - testInference.run - true - true - true - - - make - -j1 - testSymbolicSequentialSolver.run - true - false - true - - - make - -j5 - testEliminationTree.run - true - true - true - - - make - -j1 - testSymbolicBayesTree.run - true - false - true - - - make - -j2 - vSFMexample.run - true - true - true - - - make - -j5 - testInvDepthCamera3.run - true - true - true - - - make - -j5 - testTriangulation.run - true - true - true - - - make - -j2 - testVSLAMGraph - true - true - true - - - make - -j5 - check.tests - true - true - true - - - make - -j2 - timeGaussianFactorGraph.run - true - true - true - - - make - -j5 - testMarginals.run - true - true - true - - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - - - make - -j5 - testGaussianISAM2.run - true - true - true - - - make - -j5 - testSymbolicFactorGraphB.run - true - true - true - - - make - -j2 - timeSequentialOnDataset.run - true - true - true - - - make - -j5 - testGradientDescentOptimizer.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - testNonlinearOptimizer.run - true - true - true - - - make - -j2 - testGaussianBayesNet.run - true - true - true - - - make - -j2 - testNonlinearISAM.run - true - true - true - - - make - -j2 - testNonlinearEquality.run - true - true - true - - - make - -j2 - testExtendedKalmanFilter.run - true - true - true - - - make - testSimulated2DOriented.run - true - false - true - - - make - -j5 - timing.tests - true - true - true - - - make - -j5 - testNonlinearFactor.run - true - true - true - - - make - -j5 - clean - true - true - true - - - make - -j5 - testGaussianJunctionTreeB.run - true - true - true - - - make - testSimulated2D.run - true - false - true - - - make - testSimulated3D.run - true - false - true - - - make - - testSymbolicBayesNetB.run - true - false - true - - - make - -j5 - testEliminationTree.run - true - true - true - - - make - -j5 - testInference.run - true - true - true - - - make - -j5 - testKey.run - true - true - true - - - make - -j1 - testSymbolicBayesTree.run - true - false - true - - - make - -j1 - testSymbolicSequentialSolver.run - true - false - true - - - make - -j5 - testGaussianFactorGraphB.run - true - true - true - - - make - -j5 - testSummarization.run - true - true - true - - - make - -j5 - timeIncremental.run - true - true - true - make -j5 @@ -1587,7 +1033,6 @@ make - testErrors.run true false @@ -1673,38 +1118,6 @@ false true - - cpack - - -G DEB - true - false - true - - - cpack - - -G RPM - true - false - true - - - cpack - - -G TGZ - true - false - true - - - cpack - - --config CPackSourceConfig.cmake - true - true - true - make -j2 @@ -1745,14 +1158,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -1857,54 +1262,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -1913,130 +1270,66 @@ true true - + make -j5 - testStereoCamera.run + testBTree.run true true true - + make -j5 - testRot3M.run + testDSF.run true true true - + make -j5 - testPoint3.run + testDSFMap.run true true true - + make -j5 - testCalibratedCamera.run + testDSFVector.run true true true - + make -j5 - timeStereoCamera.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - timeCalibratedCamera.run - true - true - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - testCal3DS2.run - true - true - true - - - make - -j5 - testCal3Bundler.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testEssentialMatrix.run + testFixedVector.run true true true make - -j2 - all + -j5 + testGaussianISAM2.run + true + true + true + + + make + + testSimulated2DOriented.run + true + false + true + + + make + -j5 + timing.tests true true true @@ -2113,21 +1406,6 @@ true true - - make - testSimulated2DOriented.run - true - false - true - - - make - -j2 - testVSLAMConfig.run - true - true - true - make -j2 @@ -2154,6 +1432,7 @@ make + testSimulated2D.run true false @@ -2161,6 +1440,7 @@ make + testSimulated3D.run true false @@ -2174,22 +1454,46 @@ true true - + make -j5 - testVector.run + testEliminationTree.run true true true - + make -j5 - testMatrix.run + testInference.run true true true + + make + -j5 + testKey.run + true + true + true + + + make + -j1 + testSymbolicBayesTree.run + true + false + true + + + make + -j1 + testSymbolicSequentialSolver.run + true + false + true + make -j5 @@ -2278,193 +1582,10 @@ true true - - make - -j2 - SimpleRotation.run - true - true - true - - - make - -j5 - CameraResectioning.run - true - true - true - - - make - -j5 - PlanarSLAMExample.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - easyPoint2KalmanFilter.run - true - true - true - - - make - -j2 - elaboratePoint2KalmanFilter.run - true - true - true - - - make - -j5 - Pose2SLAMExample.run - true - true - true - - - make - -j2 - Pose2SLAMwSPCG_easy.run - true - true - true - - - make - -j5 - UGM_small.run - true - true - true - - - make - -j5 - LocalizationExample.run - true - true - true - - - make - -j5 - OdometryExample.run - true - true - true - - - make - -j5 - RangeISAMExample_plaza2.run - true - true - true - - - make - - testGraph.run - true - false - true - - - make - - testJunctionTree.run - true - true - true - - - make - - testSymbolicBayesNetB.run - true - true - true - - - make - -j5 - VisualISAMExample.run - true - true - true - - - make - -j5 - VisualISAM2Example.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graphviz.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graph.run - true - true - true - - - make - -j5 - SFMExample_bal.run - true - true - true - - - make - -j4 - testImuFactor.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - tests/testGaussianISAM2 - true - false - true - make - -j2 - check + -j5 + testGaussianFactorGraphB.run true true true @@ -2477,6 +1598,34 @@ true true + + cpack + -G DEB + true + false + true + + + cpack + -G RPM + true + false + true + + + cpack + -G TGZ + true + false + true + + + cpack + --config CPackSourceConfig.cmake + true + true + true + make -j3 @@ -2580,38 +1729,6 @@ true true - - make - -j5 - testGPSFactor.run - true - true - true - - - make - -j5 - testGaussMarkov1stOrderFactor.run - true - true - true - - - make - -j5 - testImplicitSchurFactor.run - true - true - true - - - make - -j2 VERBOSE=1 - check.geometry - true - false - true - make -j5 @@ -2700,38 +1817,6 @@ false true - - cpack - - -G DEB - true - false - true - - - cpack - - -G RPM - true - false - true - - - cpack - - -G TGZ - true - false - true - - - cpack - - --config CPackSourceConfig.cmake - true - false - true - make -j5 @@ -2886,11 +1971,921 @@ make + -j5 tests/testGaussianISAM2 true true true + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. + wrap + true + false + true + + + make + -j5 + testQPSolver.run + true + true + true + + + make + -j5 + testRot2.run + true + true + true + + + cmake-gui + .. + true + false + true + + + make + -j5 + testClp.run + true + true + true + + + make + -j5 + testlpsolve.run + true + true + true + + + make + -j5 + testLPSolver.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + clean all + true + true + true + + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j6 -j8 + testAntiFactor.run + true + true + true + + + make + -j6 -j8 + testBetweenFactor.run + true + true + true + + + make + -j5 + testDataset.run + true + true + true + + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j5 + testRotateFactor.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + timeCalibratedCamera.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testDiscreteFactor.run + true + true + true + + + make + -j1 + testDiscreteBayesTree.run + true + false + true + + + make + -j5 + testDiscreteFactorGraph.run + true + true + true + + + make + -j5 + testDiscreteConditional.run + true + true + true + + + make + -j5 + testDiscreteMarginals.run + true + true + true + + + make + -j2 + vSFMexample.run + true + true + true + + + make + -j5 + testInference.run + true + true + true + + + make + -j1 + testSymbolicSequentialSolver.run + true + false + true + + + make + -j5 + testEliminationTree.run + true + true + true + + + make + -j1 + testSymbolicBayesTree.run + true + false + true + + + make + -j2 + testVSLAMGraph + true + true + true + + + make + -j5 + testInvDepthCamera3.run + true + true + true + + + make + -j5 + testTriangulation.run + true + true + true + + + make + -j5 + check.tests + true + true + true + + + make + -j2 + timeGaussianFactorGraph.run + true + true + true + + + make + -j5 + testMarginals.run + true + true + true + + + make + -j5 + testSymbolicFactorGraphB.run + true + true + true + + + make + -j2 + timeSequentialOnDataset.run + true + true + true + + + make + -j5 + testGradientDescentOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + testNonlinearOptimizer.run + true + true + true + + + make + -j2 + testGaussianBayesNet.run + true + true + true + + + make + -j2 + testNonlinearISAM.run + true + true + true + + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testExtendedKalmanFilter.run + true + true + true + + + make + -j5 + testNonlinearFactor.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + testGaussianJunctionTreeB.run + true + true + true + + + make + + testSimulated2D.run + true + false + true + + + make + + testSimulated3D.run + true + false + true + + + make + testSymbolicBayesNetB.run + true + false + true + + + make + -j5 + testSummarization.run + true + true + true + + + make + -j5 + timeIncremental.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testGPSFactor.run + true + true + true + + + make + -j5 + testGaussMarkov1stOrderFactor.run + true + true + true + + + make + -j5 + testImplicitSchurFactor.run + true + true + true + + + make + -j5 + testStereoCamera.run + true + true + true + + + make + -j5 + testRot3M.run + true + true + true + + + make + -j5 + testPoint3.run + true + true + true + + + make + -j5 + testCalibratedCamera.run + true + true + true + + + make + -j5 + timeStereoCamera.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + timeCalibratedCamera.run + true + true + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + timePinholeCamera.run + true + true + true + + + make + -j5 + testCal3DS2.run + true + true + true + + + make + -j5 + testCal3Bundler.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testEssentialMatrix.run + true + true + true + + + make + -j5 + testQPSolver.run + true + true + true + + + make + -j5 + testVector.run + true + true + true + + + make + -j5 + testMatrix.run + true + true + true + + + make + -j2 + SimpleRotation.run + true + true + true + + + make + -j5 + CameraResectioning.run + true + true + true + + + make + -j5 + PlanarSLAMExample.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + easyPoint2KalmanFilter.run + true + true + true + + + make + -j2 + elaboratePoint2KalmanFilter.run + true + true + true + + + make + -j5 + Pose2SLAMExample.run + true + true + true + + + make + -j2 + Pose2SLAMwSPCG_easy.run + true + true + true + + + make + -j5 + UGM_small.run + true + true + true + + + make + -j5 + LocalizationExample.run + true + true + true + + + make + -j5 + OdometryExample.run + true + true + true + + + make + -j5 + RangeISAMExample_plaza2.run + true + true + true + + + make + testSymbolicBayesNetB.run + true + true + true + + + make + -j5 + VisualISAMExample.run + true + true + true + + + make + -j5 + VisualISAM2Example.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graphviz.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graph.run + true + true + true + + + make + -j5 + SFMExample_bal.run + true + true + true + + + make + -j4 + testImuFactor.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + + tests/testGaussianISAM2 + true + false + true + make -j2 @@ -2987,105 +2982,10 @@ true true - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make + -j5 install - testSpirit.run - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. - wrap - true - false - true - - - make - -j5 - testQPSolver.run - true - true - true - - - make - -j5 - testRot2.run - true - true - true - - - cmake-gui - .. - true - false - true - - - make - -j5 - testClp.run - true - true - true - - - make - -j5 - testlpsolve.run - true - true - true - - - make - -j5 - testLPSolver.run - true - true - true - - - make - -j5 - testSpirit.run true true true diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index 7f9991a32..377676a68 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -16,7 +16,15 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ +/// Convert a Gaussian factor to a jacobian. return empty shared ptr if failed +static JacobianFactor::shared_ptr toJacobian( + const GaussianFactor::shared_ptr& factor) { + JacobianFactor::shared_ptr jacobian( + boost::dynamic_pointer_cast(factor)); + return jacobian; +} + +//****************************************************************************** QPSolver::QPSolver(const GaussianFactorGraph& graph) : graph_(graph), fullFactorIndices_(graph) { // Split the original graph into unconstrained and constrained part @@ -38,16 +46,15 @@ QPSolver::QPSolver(const GaussianFactorGraph& graph) : } // Collect unconstrained hessians of constrained vars to build dual graph - freeHessians_ = unconstrainedHessiansOfConstrainedVars(graph, - constrainedVars); - freeHessianFactorIndex_ = VariableIndex(*freeHessians_); + findUnconstrainedHessiansOfConstrainedVars(constrainedVars); + freeHessianFactorIndex_ = VariableIndex(freeHessians_); } -/* ************************************************************************* */ -GaussianFactorGraph::shared_ptr QPSolver::unconstrainedHessiansOfConstrainedVars( - const GaussianFactorGraph& graph, const set& constrainedVars) const { - VariableIndex variableIndex(graph); - GaussianFactorGraph::shared_ptr hfg(new GaussianFactorGraph()); +//****************************************************************************** +void QPSolver::findUnconstrainedHessiansOfConstrainedVars( + const set& constrainedVars) { + VariableIndex variableIndex(graph_); + // Collect all factors involving constrained vars FastSet factors; BOOST_FOREACH(Key key, constrainedVars) { @@ -59,10 +66,12 @@ GaussianFactorGraph::shared_ptr QPSolver::unconstrainedHessiansOfConstrainedVars // Convert each factor into Hessian BOOST_FOREACH(size_t factorIndex, factors) { - if (!graph[factorIndex]) + GaussianFactor::shared_ptr gf = graph_[factorIndex]; + if (!gf) continue; // See if this is a Jacobian factor - JacobianFactor::shared_ptr jf = toJacobian(graph[factorIndex]); + JacobianFactor::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); if (jf) { // Dealing with mixed constrained factor if (jf->get_model() && jf->isConstrained()) { @@ -82,7 +91,7 @@ GaussianFactorGraph::shared_ptr QPSolver::unconstrainedHessiansOfConstrainedVars JacobianFactor::shared_ptr newJacobian = toJacobian(jf->clone()); newJacobian->setModel( noiseModel::Diagonal::Precisions(newPrecisions)); - hfg->push_back(HessianFactor(*newJacobian)); + freeHessians_.push_back(HessianFactor(*newJacobian)); } } else { // unconstrained Jacobian // Convert the original linear factor to Hessian factor @@ -93,16 +102,18 @@ GaussianFactorGraph::shared_ptr QPSolver::unconstrainedHessiansOfConstrainedVars // because of a weird error which might be related to clang // See this: https://groups.google.com/forum/#!topic/ceres-solver/DYhqOLPquHU // My current way to fix this is to compile both gtsam and my library in Release mode - hfg->add(HessianFactor(*jf)); + freeHessians_.add(HessianFactor(*jf)); } } else { // If it's not a Jacobian, it should be a hessian factor. Just add! - hfg->push_back(graph[factorIndex]); + HessianFactor::shared_ptr hf = // + boost::dynamic_pointer_cast(gf); + if (hf) + freeHessians_.push_back(hf); } } - return hfg; } -/* ************************************************************************* */ +//****************************************************************************** GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, const VectorValues& x0, bool useLeastSquare) const { static const bool debug = false; @@ -122,8 +133,7 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, // Find xi's dim from the first factor on xi if (xiFactors.size() == 0) continue; - GaussianFactor::shared_ptr xiFactor0 = freeHessians_->at( - *xiFactors.begin()); + GaussianFactor::shared_ptr xiFactor0 = freeHessians_.at(*xiFactors.begin()); size_t xiDim = xiFactor0->getDim(xiFactor0->find(xiKey)); if (debug) xiFactor0->print("xiFactor0: "); @@ -131,12 +141,12 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, cout << "xiKey: " << string(Symbol(xiKey)) << ", xiDim: " << xiDim << endl; - //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Compute the b-vector for the dual factor Ax-b // b = gradf(xi) = \frac{\partial f}{\partial xi}' = \sum_j G_ij*xj - gi Vector gradf_xi = zero(xiDim); BOOST_FOREACH(size_t factorIx, xiFactors) { - HessianFactor::shared_ptr factor = toHessian(freeHessians_->at(factorIx)); + HessianFactor::shared_ptr factor = freeHessians_.at(factorIx); Factor::const_iterator xi = factor->find(xiKey); // Sum over Gij*xj for all xj connecting to xi for (Factor::const_iterator xj = factor->begin(); xj != factor->end(); @@ -158,7 +168,7 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, gradf_xi += -factor->linearTerm(xi); } - //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Compute the Jacobian A for the dual factor Ax-b // Obtain the jacobians for lambda variables from their corresponding constraints // A = gradc_k(xi) = \frac{\partial c_k}{\partial xi}' @@ -191,7 +201,7 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, lambdaTerms.push_back(make_pair(factorIndex, A_k)); } - //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Create and add factors to the dual graph // If least square approximation is desired, use unit noise model. if (debug) @@ -232,7 +242,7 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, return dualGraph; } -/* ************************************************************************* */ +//****************************************************************************** pair QPSolver::findWorstViolatedActiveIneq( const VectorValues& lambdas) const { int worstFactorIx = -1, worstSigmaIx = -1; @@ -253,9 +263,9 @@ pair QPSolver::findWorstViolatedActiveIneq( return make_pair(worstFactorIx, worstSigmaIx); } -/* ************************************************************************* */bool QPSolver::updateWorkingSetInplace( - GaussianFactorGraph& workingGraph, int factorIx, int sigmaIx, - double newSigma) const { +//****************************************************************************** +bool QPSolver::updateWorkingSetInplace(GaussianFactorGraph& workingGraph, + int factorIx, int sigmaIx, double newSigma) const { if (factorIx < 0 || sigmaIx < 0) return false; Vector sigmas = toJacobian(workingGraph.at(factorIx))->get_model()->sigmas(); @@ -264,7 +274,7 @@ pair QPSolver::findWorstViolatedActiveIneq( return true; } -/* ************************************************************************* */ +//****************************************************************************** /* We have to make sure the new solution with alpha satisfies all INACTIVE ineq constraints * If some inactive ineq constraints complain about the full step (alpha = 1), * we have to adjust alpha to stay within the ineq constraints' feasible regions. @@ -337,9 +347,9 @@ boost::tuple QPSolver::computeStepSize( return boost::make_tuple(minAlpha, closestFactorIx, closestSigmaIx); } -/* ************************************************************************* */bool QPSolver::iterateInPlace( - GaussianFactorGraph& workingGraph, VectorValues& currentSolution, - VectorValues& lambdas) const { +//****************************************************************************** +bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, + VectorValues& currentSolution, VectorValues& lambdas) const { static bool debug = false; if (debug) workingGraph.print("workingGraph: "); @@ -400,7 +410,7 @@ boost::tuple QPSolver::computeStepSize( return false; } -/* ************************************************************************* */ +//****************************************************************************** pair QPSolver::optimize( const VectorValues& initials) const { GaussianFactorGraph workingGraph = graph_.clone(); @@ -413,7 +423,7 @@ pair QPSolver::optimize( return make_pair(currentSolution, lambdas); } -/* ************************************************************************* */ +//****************************************************************************** pair QPSolver::initialValuesLP() const { size_t firstSlackKey = 0; BOOST_FOREACH(Key key, fullFactorIndices_ | boost::adaptors::map_keys) { @@ -455,7 +465,7 @@ pair QPSolver::initialValuesLP() const { return make_pair(initials, firstSlackKey); } -/* ************************************************************************* */ +//****************************************************************************** VectorValues QPSolver::objectiveCoeffsLP(Key firstSlackKey) const { VectorValues slackObjective; for (size_t i = 0; i < constraintIndices_.size(); ++i) { @@ -474,7 +484,7 @@ VectorValues QPSolver::objectiveCoeffsLP(Key firstSlackKey) const { return slackObjective; } -/* ************************************************************************* */ +//****************************************************************************** pair QPSolver::constraintsLP( Key firstSlackKey) const { // Create constraints and 0 lower bounds (zi>=0) @@ -504,7 +514,7 @@ pair QPSolver::constraintsLP( return make_pair(constraints, slackLowerBounds); } -/* ************************************************************************* */ +//****************************************************************************** pair QPSolver::findFeasibleInitialValues() const { static const bool debug = false; // Initial values with slack variables for the LP subproblem, Nocedal06book, pg.473 @@ -554,7 +564,7 @@ pair QPSolver::findFeasibleInitialValues() const { return make_pair(slackSum < 1e-5, solution); } -/* ************************************************************************* */ +//****************************************************************************** pair QPSolver::optimize() const { bool isFeasible; VectorValues initials; diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index 947afaddf..8e9b7de76 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -23,9 +23,13 @@ namespace gtsam { * and a positive sigma denotes a normal Gaussian noise model. */ class QPSolver { + + class Hessians: public FactorGraph { + }; + const GaussianFactorGraph& graph_; //!< the original graph, can't be modified! FastVector constraintIndices_; //!< Indices of constrained factors in the original graph - GaussianFactorGraph::shared_ptr freeHessians_; //!< unconstrained Hessians of constrained variables + Hessians freeHessians_; //!< unconstrained Hessians of constrained variables VariableIndex freeHessianFactorIndex_; //!< indices of unconstrained Hessian factors of constrained variables // gtsam calls it "VariableIndex", but I think FactorIndex // makes more sense, because it really stores factor indices. @@ -43,7 +47,7 @@ public: } /// Return the Hessian factor graph of constrained variables - GaussianFactorGraph::shared_ptr freeHessiansOfConstrainedVars() const { + const Hessians& freeHessiansOfConstrainedVars() const { return freeHessians_; } @@ -172,29 +176,11 @@ public: /// Find a feasible initial point std::pair findFeasibleInitialValues() const; - /// Convert a Gaussian factor to a jacobian. return empty shared ptr if failed - /// TODO: Move to GaussianFactor? - static JacobianFactor::shared_ptr toJacobian( - const GaussianFactor::shared_ptr& factor) { - JacobianFactor::shared_ptr jacobian( - boost::dynamic_pointer_cast(factor)); - return jacobian; - } - - /// Convert a Gaussian factor to a Hessian. Return empty shared ptr if failed - /// TODO: Move to GaussianFactor? - static HessianFactor::shared_ptr toHessian( - const GaussianFactor::shared_ptr factor) { - HessianFactor::shared_ptr hessian( - boost::dynamic_pointer_cast(factor)); - return hessian; - } - private: + /// Collect all free Hessians involving constrained variables into a graph - GaussianFactorGraph::shared_ptr unconstrainedHessiansOfConstrainedVars( - const GaussianFactorGraph& graph, - const std::set& constrainedVars) const; + void findUnconstrainedHessiansOfConstrainedVars( + const std::set& constrainedVars); }; diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index a210c6464..ec7edac60 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -89,13 +89,9 @@ TEST(QPSolver, constraintsAux) { LONGS_EQUAL(-1, factorIx2); LONGS_EQUAL(-1, lambdaIx2); - GaussianFactorGraph::shared_ptr freeHessian = - solver.freeHessiansOfConstrainedVars(); - GaussianFactorGraph expectedFreeHessian; - expectedFreeHessian.push_back( - HessianFactor(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1), - 2.0 * ones(1, 1), zero(1), 1.0)); - EXPECT(expectedFreeHessian.equals(*freeHessian)); + HessianFactor expectedFreeHessian(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), + 3.0 * ones(1), 2.0 * ones(1, 1), zero(1), 1.0); + EXPECT(solver.freeHessiansOfConstrainedVars()[0]->equals(expectedFreeHessian)); } /* ************************************************************************* */ From 4871202664a4507d328f7f39ae87a4413cf6299c Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 27 Nov 2014 10:49:35 +0100 Subject: [PATCH 02/13] identifyLeavingConstraint --- gtsam_unstable/linear/QPSolver.cpp | 4 ++-- gtsam_unstable/linear/QPSolver.h | 2 +- gtsam_unstable/linear/tests/testQPSolver.cpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index 377676a68..e951deb5e 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -243,7 +243,7 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, } //****************************************************************************** -pair QPSolver::findWorstViolatedActiveIneq( +pair QPSolver::identifyLeavingConstraint( const VectorValues& lambdas) const { int worstFactorIx = -1, worstSigmaIx = -1; // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either @@ -371,7 +371,7 @@ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, lambdas.print("lambdas :"); int factorIx, sigmaIx; - boost::tie(factorIx, sigmaIx) = findWorstViolatedActiveIneq(lambdas); + boost::tie(factorIx, sigmaIx) = identifyLeavingConstraint(lambdas); if (debug) cout << "violated active ineq - factorIx, sigmaIx: " << factorIx << " " << sigmaIx << endl; diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index 8e9b7de76..196eeadc3 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -115,7 +115,7 @@ public: * And we want to remove the worst one with the largest lambda from the active set. * */ - std::pair findWorstViolatedActiveIneq( + std::pair identifyLeavingConstraint( const VectorValues& lambdas) const; /** diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index ec7edac60..d34a5aff6 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -77,14 +77,14 @@ TEST(QPSolver, constraintsAux) { VectorValues lambdas; lambdas.insert(constraintIx[0], (Vector(4) << -0.5, 0.0, 0.3, 0.1)); int factorIx, lambdaIx; - boost::tie(factorIx, lambdaIx) = solver.findWorstViolatedActiveIneq(lambdas); + boost::tie(factorIx, lambdaIx) = solver.identifyLeavingConstraint(lambdas); LONGS_EQUAL(1, factorIx); LONGS_EQUAL(2, lambdaIx); VectorValues lambdas2; lambdas2.insert(constraintIx[0], (Vector(4) << -0.5, 0.0, -0.3, -0.1)); int factorIx2, lambdaIx2; - boost::tie(factorIx2, lambdaIx2) = solver.findWorstViolatedActiveIneq( + boost::tie(factorIx2, lambdaIx2) = solver.identifyLeavingConstraint( lambdas2); LONGS_EQUAL(-1, factorIx2); LONGS_EQUAL(-1, lambdaIx2); From 3800e1f10181a22f20cd81a83708e166fd4942f6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 27 Nov 2014 10:52:25 +0100 Subject: [PATCH 03/13] initials -> initialValues --- gtsam_unstable/linear/QPSolver.cpp | 26 +++++------ gtsam_unstable/linear/QPSolver.h | 2 +- gtsam_unstable/linear/tests/testQPSolver.cpp | 48 ++++++++++---------- 3 files changed, 38 insertions(+), 38 deletions(-) diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index e951deb5e..b94ba9a15 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -412,9 +412,9 @@ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, //****************************************************************************** pair 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 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 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 QPSolver::constraintsLP( pair 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 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 QPSolver::findFeasibleInitialValues() const { //****************************************************************************** pair 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 */ diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index 196eeadc3..ea4c18fa9 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -150,7 +150,7 @@ public: * @return a pair of solutions */ std::pair 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 diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index d34a5aff6..6d8e07901 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -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)); From 88693e21296853d83da723efc279bef81dbfd334 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 27 Nov 2014 11:01:45 +0100 Subject: [PATCH 04/13] Comments (ineq -> inequality) --- gtsam_unstable/linear/QPSolver.cpp | 33 +++++++++++++++--------------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index b94ba9a15..ac6bc549c 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -81,7 +81,7 @@ void QPSolver::findUnconstrainedHessiansOfConstrainedVars( bool mixed = false; for (size_t s = 0; s < sigmas.size(); ++s) { if (sigmas[s] <= 1e-9) - newPrecisions[s] = 0.0; // 0 info for constraints (both ineq and eq) + newPrecisions[s] = 0.0; // 0 info for constraints (both inequality and eq) else { newPrecisions[s] = 1.0 / sigmas[s]; mixed = true; @@ -188,7 +188,7 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, // Deal with mixed sigmas: no information if sigma != 0 Vector sigmas = factor->get_model()->sigmas(); for (size_t sigmaIx = 0; sigmaIx < sigmas.size(); ++sigmaIx) { - // if it's either ineq (sigma<0) or unconstrained (sigma>0) + // if it's either inequality (sigma<0) or unconstrained (sigma>0) // we have no information about it if (fabs(sigmas[sigmaIx]) > 1e-9) { A_k.col(sigmaIx) = zero(A_k.rows()); @@ -247,7 +247,7 @@ pair QPSolver::identifyLeavingConstraint( const VectorValues& lambdas) const { int worstFactorIx = -1, worstSigmaIx = -1; // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either - // inactive or a good ineq constraint, so we don't care! + // inactive or a good inequality constraint, so we don't care! double maxLambda = 0.0; BOOST_FOREACH(size_t factorIx, constraintIndices_) { Vector lambda = lambdas.at(factorIx); @@ -275,12 +275,12 @@ bool QPSolver::updateWorkingSetInplace(GaussianFactorGraph& workingGraph, } //****************************************************************************** -/* We have to make sure the new solution with alpha satisfies all INACTIVE ineq constraints - * If some inactive ineq constraints complain about the full step (alpha = 1), - * we have to adjust alpha to stay within the ineq constraints' feasible regions. +/* We have to make sure the new solution with alpha satisfies all INACTIVE inequality constraints + * If some inactive inequality constraints complain about the full step (alpha = 1), + * we have to adjust alpha to stay within the inequality constraints' feasible regions. * - * For each inactive ineq j: - * - We already have: aj'*xk - bj <= 0, since xk satisfies all ineq constraints + * For each inactive inequality j: + * - We already have: aj'*xk - bj <= 0, since xk satisfies all inequality constraints * - We want: aj'*(xk + alpha*p) - bj <= 0 * - If aj'*p <= 0, we have: aj'*(xk + alpha*p) <= aj'*xk <= bj, for all alpha>0 * it's good! @@ -288,7 +288,7 @@ bool QPSolver::updateWorkingSetInplace(GaussianFactorGraph& workingGraph, * aj'*xk + alpha*aj'*p - bj <= 0 --> alpha <= (bj - aj'*xk) / (aj'*p) * We want to step as far as possible, so we should choose alpha = (bj - aj'*xk) / (aj'*p) * - * We want the minimum of all those alphas among all inactive ineq. + * We want the minimum of all those alphas among all inactive inequality. */ boost::tuple QPSolver::computeStepSize( const GaussianFactorGraph& workingGraph, const VectorValues& xk, @@ -373,23 +373,24 @@ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, int factorIx, sigmaIx; boost::tie(factorIx, sigmaIx) = identifyLeavingConstraint(lambdas); if (debug) - cout << "violated active ineq - factorIx, sigmaIx: " << factorIx << " " - << sigmaIx << endl; + cout << "violated active inequality - factorIx, sigmaIx: " << factorIx + << " " << sigmaIx << endl; - // Try to disactivate the weakest violated ineq constraints - // if not successful, i.e. all ineq constraints are satisfied: We have the solution!! + // Try to de-activate the weakest violated inequality constraints + // if not successful, i.e. all inequality constraints are satisfied: + // We have the solution!! if (!updateWorkingSetInplace(workingGraph, factorIx, sigmaIx, -1.0)) return true; } else { // If we CAN make some progress - // Adapt stepsize if some inactive inequality constraints complain about this move + // Adapt stepsize if some inactive constraints complain about this move if (debug) cout << "Computing stepsize..." << endl; double alpha; int factorIx, sigmaIx; VectorValues p = newSolution - currentSolution; - boost::tie(alpha, factorIx, sigmaIx) = computeStepSize(workingGraph, - currentSolution, p); + boost::tie(alpha, factorIx, sigmaIx) = // + computeStepSize(workingGraph, currentSolution, p); if (debug) cout << "alpha, factorIx, sigmaIx: " << alpha << " " << factorIx << " " << sigmaIx << endl; From 3aa7fd6d1829dbbf7cbb96a3398f7d05668d8def Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Thu, 27 Nov 2014 10:45:23 -0500 Subject: [PATCH 05/13] add LinearConstraint --- gtsam_unstable/linear/LinearConstraint.h | 124 +++++++++ .../linear/tests/testLinearConstraint.cpp | 237 ++++++++++++++++++ 2 files changed, 361 insertions(+) create mode 100644 gtsam_unstable/linear/LinearConstraint.h create mode 100644 gtsam_unstable/linear/tests/testLinearConstraint.cpp diff --git a/gtsam_unstable/linear/LinearConstraint.h b/gtsam_unstable/linear/LinearConstraint.h new file mode 100644 index 000000000..8cb190016 --- /dev/null +++ b/gtsam_unstable/linear/LinearConstraint.h @@ -0,0 +1,124 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * LinearConstraint.h + * @brief: LinearConstraint derived from Base with constrained noise model + * @date: Nov 27, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * This class defines Linear constraints by inherit Base + * with the special Constrained noise model + */ +class LinearConstraint: public JacobianFactor { +public: + typedef LinearConstraint This; ///< Typedef to this class + typedef JacobianFactor Base; ///< Typedef to base class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + +public: + /** default constructor for I/O */ + LinearConstraint() : Base() {} + + /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ + explicit LinearConstraint(const HessianFactor& hf) { + throw std::runtime_error("Cannot convert HessianFactor to LinearConstraint"); + } + + /** Construct unary factor */ + LinearConstraint(Key i1, const Matrix& A1, const Vector& b) : + Base(i1, A1, b, noiseModel::Constrained::All(b.rows())) { + } + + /** Construct binary factor */ + LinearConstraint(Key i1, const Matrix& A1, Key i2, const Matrix& A2, + const Vector& b) : + Base(i1, A1, i2, A2, b, noiseModel::Constrained::All(b.rows())) { + } + + /** Construct ternary factor */ + LinearConstraint(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, const Vector& b) : + Base(i1, A1, i2, A2, i3, A3, b, + noiseModel::Constrained::All(b.rows())) { + } + + /** Construct four-ary factor */ + LinearConstraint(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, Key i4, const Matrix& A4, const Vector& b) : + Base(i1, A1, i2, A2, i3, A3, i4, A4, b, + noiseModel::Constrained::All(b.rows())) { + } + + /** Construct five-ary factor */ + LinearConstraint(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, Key i4, const Matrix& A4, Key i5, const Matrix& A5, + const Vector& b) : + Base(i1, A1, i2, A2, i3, A3, i4, A4, i5, A5, b, + noiseModel::Constrained::All(b.rows())) { + } + + /** Construct six-ary factor */ + LinearConstraint(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, Key i4, const Matrix& A4, Key i5, const Matrix& A5, + Key i6, const Matrix& A6, const Vector& b) : + Base(i1, A1, i2, A2, i3, A3, i4, A4, i5, A5, i6, A6, b, + noiseModel::Constrained::All(b.rows())) { + } + + /** Construct an n-ary factor + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the factor. */ + template + LinearConstraint(const TERMS& terms, const Vector& b) : + Base(terms, b, noiseModel::Constrained::All(b.rows())) { + } + + /** Virtual destructor */ + virtual ~LinearConstraint() { + } + + // Testable interface + virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const { + return Base::equals(lf, tol); + } + + /** Clone this LinearConstraint */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + boost::make_shared(*this)); + } + + /** Special error_vector for constraints (A*x-b) */ + Vector error_vector(const VectorValues& c) const { + return unweighted_error(c); + } + + /** Special error for constraints. + * I think it should be zero, as this function is meant for objective cost. + * But the name "error" can be misleading. + * TODO: confirm with Frank!! */ + virtual double error(const VectorValues& c) const { + return 0.0; + } + +}; // LinearConstraint + +} // gtsam + diff --git a/gtsam_unstable/linear/tests/testLinearConstraint.cpp b/gtsam_unstable/linear/tests/testLinearConstraint.cpp new file mode 100644 index 000000000..489af1db3 --- /dev/null +++ b/gtsam_unstable/linear/tests/testLinearConstraint.cpp @@ -0,0 +1,237 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testLinearConstraint.cpp + * @brief Unit tests for LinearConstraint + * @author thduynguyen + **/ + +#include +#include +#include +#include +#include + +#include +#include +//#include +//#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +namespace { + namespace simple { + // Terms we'll use + const vector > terms = list_of > + (make_pair(5, Matrix3::Identity())) + (make_pair(10, 2*Matrix3::Identity())) + (make_pair(15, 3*Matrix3::Identity())); + + // RHS and sigmas + const Vector b = (Vector(3) << 1., 2., 3.); + const SharedDiagonal noise = noiseModel::Constrained::All(3); + } +} + +/* ************************************************************************* */ +TEST(LinearConstraint, constructors_and_accessors) +{ + using namespace simple; + + // Test for using different numbers of terms + { + // One term constructor + LinearConstraint expected( + boost::make_iterator_range(terms.begin(), terms.begin() + 1), b); + LinearConstraint actual(terms[0].first, terms[0].second, b); + EXPECT(assert_equal(expected, actual)); + LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back()); + EXPECT(assert_equal(terms[0].second, actual.getA(actual.end() - 1))); + EXPECT(assert_equal(b, expected.getb())); + EXPECT(assert_equal(b, actual.getb())); + EXPECT(assert_equal(*noise, *actual.get_model())); + } + { + // Two term constructor + LinearConstraint expected( + boost::make_iterator_range(terms.begin(), terms.begin() + 2), b); + LinearConstraint actual(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, b); + EXPECT(assert_equal(expected, actual)); + LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back()); + EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1))); + EXPECT(assert_equal(b, expected.getb())); + EXPECT(assert_equal(b, actual.getb())); + EXPECT(assert_equal(*noise, *actual.get_model())); + } + { + // Three term constructor + LinearConstraint expected( + boost::make_iterator_range(terms.begin(), terms.begin() + 3), b); + LinearConstraint actual(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b); + EXPECT(assert_equal(expected, actual)); + LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); + EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); + EXPECT(assert_equal(b, expected.getb())); + EXPECT(assert_equal(b, actual.getb())); + EXPECT(assert_equal(*noise, *actual.get_model())); + } +} + +/* ************************************************************************* */ +TEST(LinearConstraint, Hessian_conversion) { + HessianFactor hessian(0, (Matrix(4,4) << + 1.57, 2.695, -1.1, -2.35, + 2.695, 11.3125, -0.65, -10.225, + -1.1, -0.65, 1, 0.5, + -2.35, -10.225, 0.5, 9.25), + (Vector(4) << -7.885, -28.5175, 2.75, 25.675), + 73.1725); + + try { + LinearConstraint actual(hessian); + EXPECT(false); + } + catch (const std::runtime_error& exception) { + EXPECT(true); + } +} + +/* ************************************************************************* */ +TEST(LinearConstraint, error) +{ + LinearConstraint factor(simple::terms, simple::b); + + VectorValues values; + values.insert(5, Vector::Constant(3, 1.0)); + values.insert(10, Vector::Constant(3, 0.5)); + values.insert(15, Vector::Constant(3, 1.0/3.0)); + + Vector expected_unwhitened(3); expected_unwhitened << 2.0, 1.0, 0.0; + Vector actual_unwhitened = factor.unweighted_error(values); + EXPECT(assert_equal(expected_unwhitened, actual_unwhitened)); + + // whitened is meaningless in constraints + Vector expected_whitened(3); expected_whitened = expected_unwhitened; + Vector actual_whitened = factor.error_vector(values); + EXPECT(assert_equal(expected_whitened, actual_whitened)); + + double expected_error = 0.0; + double actual_error = factor.error(values); + DOUBLES_EQUAL(expected_error, actual_error, 1e-10); +} + +/* ************************************************************************* */ +TEST(LinearConstraint, matrices_NULL) +{ + // Make sure everything works with NULL noise model + LinearConstraint factor(simple::terms, simple::b); + + Matrix AExpected(3, 9); + AExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second; + Vector rhsExpected = simple::b; + Matrix augmentedJacobianExpected(3, 10); + augmentedJacobianExpected << AExpected, rhsExpected; + + // Whitened Jacobian + EXPECT(assert_equal(AExpected, factor.jacobian().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobian().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobian())); + + // Unwhitened Jacobian + EXPECT(assert_equal(AExpected, factor.jacobianUnweighted().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted())); +} + +/* ************************************************************************* */ +TEST(LinearConstraint, matrices) +{ + // And now witgh a non-unit noise model + LinearConstraint factor(simple::terms, simple::b); + + Matrix jacobianExpected(3, 9); + jacobianExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second; + Vector rhsExpected = simple::b; + Matrix augmentedJacobianExpected(3, 10); + augmentedJacobianExpected << jacobianExpected, rhsExpected; + + Matrix augmentedHessianExpected = + augmentedJacobianExpected.transpose() * simple::noise->R().transpose() + * simple::noise->R() * augmentedJacobianExpected; + + // Whitened Jacobian + EXPECT(assert_equal(jacobianExpected, factor.jacobian().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobian().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobian())); + + // Unwhitened Jacobian + EXPECT(assert_equal(jacobianExpected, factor.jacobianUnweighted().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted())); +} + +/* ************************************************************************* */ +TEST(LinearConstraint, operators ) +{ + Matrix I = eye(2); + Vector b = (Vector(2) << 0.2,-0.1); + LinearConstraint lf(1, -I, 2, I, b); + + VectorValues c; + c.insert(1, (Vector(2) << 10.,20.)); + c.insert(2, (Vector(2) << 30.,60.)); + + // test A*x + Vector expectedE = (Vector(2) << 20.,40.); + Vector actualE = lf * c; + EXPECT(assert_equal(expectedE, actualE)); + + // test A^e + VectorValues expectedX; + expectedX.insert(1, (Vector(2) << -20.,-40.)); + expectedX.insert(2, (Vector(2) << 20., 40.)); + VectorValues actualX = VectorValues::Zero(expectedX); + lf.transposeMultiplyAdd(1.0, actualE, actualX); + EXPECT(assert_equal(expectedX, actualX)); + + // test gradient at zero + Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian(); + VectorValues expectedG; + expectedG.insert(1, (Vector(2) << -1,-1)); + expectedG.insert(2, (Vector(2) << 1, 1)); + VectorValues actualG = lf.gradientAtZero(); + EXPECT(assert_equal(expectedG, actualG)); +} + +/* ************************************************************************* */ +TEST(LinearConstraint, default_error ) +{ + LinearConstraint f; + double actual = f.error(VectorValues()); + DOUBLES_EQUAL(0.0, actual, 1e-15); +} + +//* ************************************************************************* */ +TEST(LinearConstraint, empty ) +{ + // create an empty factor + LinearConstraint f; + EXPECT(f.empty()); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ From a4f8ead2a68c30eb5151f505e2cb2f0685cd0ce6 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Tue, 9 Dec 2014 06:12:41 -0500 Subject: [PATCH 06/13] add print --- gtsam_unstable/linear/LinearConstraint.h | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/gtsam_unstable/linear/LinearConstraint.h b/gtsam_unstable/linear/LinearConstraint.h index 8cb190016..365c06688 100644 --- a/gtsam_unstable/linear/LinearConstraint.h +++ b/gtsam_unstable/linear/LinearConstraint.h @@ -94,11 +94,17 @@ public: virtual ~LinearConstraint() { } - // Testable interface + /** equals */ virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const { return Base::equals(lf, tol); } + /** print */ + virtual void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const { + Base::print(s, formatter); + } + /** Clone this LinearConstraint */ virtual GaussianFactor::shared_ptr clone() const { return boost::static_pointer_cast( From 8c4705b90520a6be07fcf83e2df474524a16f2e3 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Tue, 9 Dec 2014 06:13:01 -0500 Subject: [PATCH 07/13] check testable concept --- gtsam_unstable/linear/tests/testLinearConstraint.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/linear/tests/testLinearConstraint.cpp b/gtsam_unstable/linear/tests/testLinearConstraint.cpp index 489af1db3..7aaec5720 100644 --- a/gtsam_unstable/linear/tests/testLinearConstraint.cpp +++ b/gtsam_unstable/linear/tests/testLinearConstraint.cpp @@ -23,13 +23,13 @@ #include #include -//#include -//#include using namespace std; using namespace gtsam; using namespace boost::assign; +GTSAM_CONCEPT_TESTABLE_INST(LinearConstraint) + namespace { namespace simple { // Terms we'll use From 001794ac84f82deb2085bbec8ae9f36986932110 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Tue, 9 Dec 2014 06:13:57 -0500 Subject: [PATCH 08/13] refactor QPSolver inprogress... Compiled but tests failed. --- gtsam_unstable/linear/LPSolver.cpp | 45 +- gtsam_unstable/linear/LPSolver.h | 27 +- gtsam_unstable/linear/LinearEquality.h | 140 +++++ .../linear/LinearEqualityFactorGraph.h | 32 + gtsam_unstable/linear/LinearInequality.h | 152 +++++ .../linear/LinearInequalityFactorGraph.h | 48 ++ gtsam_unstable/linear/QP.h | 58 ++ gtsam_unstable/linear/QPSolver.cpp | 595 +++++++----------- gtsam_unstable/linear/QPSolver.h | 115 ++-- gtsam_unstable/linear/tests/testQPSolver.cpp | 288 ++++----- 10 files changed, 912 insertions(+), 588 deletions(-) create mode 100644 gtsam_unstable/linear/LinearEquality.h create mode 100644 gtsam_unstable/linear/LinearEqualityFactorGraph.h create mode 100644 gtsam_unstable/linear/LinearInequality.h create mode 100644 gtsam_unstable/linear/LinearInequalityFactorGraph.h create mode 100644 gtsam_unstable/linear/QP.h diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp index e12fa477e..4c85a1f5c 100644 --- a/gtsam_unstable/linear/LPSolver.cpp +++ b/gtsam_unstable/linear/LPSolver.cpp @@ -28,21 +28,30 @@ void LPSolver::buildMetaInformation() { freeVars_.insert(key); } // Now collect remaining keys in constraints - VariableIndex factorIndex(*constraints_); + VariableIndex factorIndex(*equalities_); BOOST_FOREACH(Key key, factorIndex | boost::adaptors::map_keys) { if (!variableColumnNo_.count(key)) { - JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast< - JacobianFactor>(constraints_->at(*factorIndex[key].begin())); - if (!jacobian || !jacobian->isConstrained()) { - throw runtime_error("Invalid constrained graph!"); - } - size_t dim = jacobian->getDim(jacobian->find(key)); + LinearEquality::shared_ptr factor = equalities_->at(*factorIndex[key].begin()); + size_t dim = factor->getDim(factor->find(key)); variableColumnNo_.insert(make_pair(key, firstVarIndex)); variableDims_.insert(make_pair(key, dim)); firstVarIndex += variableDims_[key]; freeVars_.insert(key); } } + + VariableIndex factorIndex2(*inequalities_); + BOOST_FOREACH(Key key, factorIndex2 | boost::adaptors::map_keys) { + if (!variableColumnNo_.count(key)) { + LinearInequality::shared_ptr factor = inequalities_->at(*factorIndex2[key].begin()); + size_t dim = factor->getDim(factor->find(key)); + variableColumnNo_.insert(make_pair(key, firstVarIndex)); + variableDims_.insert(make_pair(key, dim)); + firstVarIndex += variableDims_[key]; + freeVars_.insert(key); + } + } + // Collect the remaining keys in lowerBounds BOOST_FOREACH(Key key, lowerBounds_ | boost::adaptors::map_keys) { if (!variableColumnNo_.count(key)) { @@ -67,7 +76,7 @@ void LPSolver::buildMetaInformation() { /* ************************************************************************* */ void LPSolver::addConstraints(const boost::shared_ptr& lp, - const JacobianFactor::shared_ptr& jacobian) const { + const JacobianFactor::shared_ptr& jacobian, int constraintType) const { if (!jacobian || !jacobian->isConstrained()) throw runtime_error("LP only accepts constrained factors!"); @@ -76,7 +85,6 @@ void LPSolver::addConstraints(const boost::shared_ptr& lp, vector columnNo = buildColumnNo(keys); // Add each row to lp one by one. TODO: is there a faster way? - Vector sigmas = jacobian->get_model()->sigmas(); Matrix A = jacobian->getA(); Vector b = jacobian->getb(); for (int i = 0; i < A.rows(); ++i) { @@ -88,11 +96,6 @@ void LPSolver::addConstraints(const boost::shared_ptr& lp, // so we have to make a new copy for every row!!!!! vector columnNoCopy(columnNo); - if (sigmas[i] > 0) { - cout << "Warning: Ignore Gaussian noise (sigma>0) in LP constraints!" - << endl; - } - int constraintType = (sigmas[i] < 0) ? LE : EQ; if (!add_constraintex(lp.get(), columnNoCopy.size(), r.data(), columnNoCopy.data(), constraintType, b[i])) throw runtime_error("LP can't accept Gaussian noise!"); @@ -132,13 +135,17 @@ boost::shared_ptr LPSolver::buildModel() const { // Makes building the model faster if it is done rows by row set_add_rowmode(lp.get(), TRUE); - // Add constraints - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, *constraints_) { - JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast< - JacobianFactor>(factor); - addConstraints(lp, jacobian); + // Add equality constraints + BOOST_FOREACH(const LinearEquality::shared_ptr& factor, *equalities_) { + addConstraints(lp, factor, EQ); } + // Add inequality constraints + BOOST_FOREACH(const LinearInequality::shared_ptr& factor, *inequalities_) { + addConstraints(lp, factor, LE); + } + + // Add bounds addBounds(lp); diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index 73382fe3f..99bf54450 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -7,9 +7,10 @@ #pragma once -#include #include #include +#include +#include #include @@ -25,7 +26,8 @@ namespace gtsam { */ class LPSolver { VectorValues objectiveCoeffs_; - GaussianFactorGraph::shared_ptr constraints_; + LinearEqualityFactorGraph::shared_ptr equalities_; + LinearInequalityFactorGraph::shared_ptr inequalities_; VectorValues lowerBounds_, upperBounds_; std::map variableColumnNo_, variableDims_; size_t nrColumns_; @@ -38,11 +40,12 @@ public: * set as unbounded, i.e. -inf <= x <= inf. */ LPSolver(const VectorValues& objectiveCoeffs, - const GaussianFactorGraph::shared_ptr& constraints, + const LinearEqualityFactorGraph::shared_ptr& equalities, + const LinearInequalityFactorGraph::shared_ptr& inequalities, const VectorValues& lowerBounds = VectorValues(), const VectorValues& upperBounds = VectorValues()) : - objectiveCoeffs_(objectiveCoeffs), constraints_(constraints), lowerBounds_( - lowerBounds), upperBounds_(upperBounds) { + objectiveCoeffs_(objectiveCoeffs), equalities_(equalities), inequalities_( + inequalities), lowerBounds_(lowerBounds), upperBounds_(upperBounds) { buildMetaInformation(); } @@ -73,18 +76,18 @@ public: template std::vector buildColumnNo(const KEYLIST& keyList) const { std::vector columnNo; - BOOST_FOREACH(Key key, keyList) { - std::vector varIndices = boost::copy_range >( - boost::irange(variableColumnNo_.at(key), - variableColumnNo_.at(key) + variableDims_.at(key))); - columnNo.insert(columnNo.end(), varIndices.begin(), varIndices.end()); - } + BOOST_FOREACH(Key key, keyList){ + std::vector varIndices = boost::copy_range >( + boost::irange(variableColumnNo_.at(key), + variableColumnNo_.at(key) + variableDims_.at(key))); + columnNo.insert(columnNo.end(), varIndices.begin(), varIndices.end()); + } return columnNo; } /// Add all [scalar] constraints in a constrained Jacobian factor to lp void addConstraints(const boost::shared_ptr& lp, - const JacobianFactor::shared_ptr& jacobian) const; + const JacobianFactor::shared_ptr& jacobian, int type) const; /** * Add all bounds to lp. diff --git a/gtsam_unstable/linear/LinearEquality.h b/gtsam_unstable/linear/LinearEquality.h new file mode 100644 index 000000000..ba80e16bf --- /dev/null +++ b/gtsam_unstable/linear/LinearEquality.h @@ -0,0 +1,140 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * LinearEquality.h + * @brief: LinearEquality derived from Base with constrained noise model + * @date: Nov 27, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * This class defines Linear constraints by inherit Base + * with the special Constrained noise model + */ +class LinearEquality: public JacobianFactor { +public: + typedef LinearEquality This; ///< Typedef to this class + typedef JacobianFactor Base; ///< Typedef to base class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + +private: + Key dualKey_; + +public: + /** default constructor for I/O */ + LinearEquality() : + Base() { + } + + /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ + explicit LinearEquality(const HessianFactor& hf) { + throw std::runtime_error("Cannot convert HessianFactor to LinearEquality"); + } + + /** Construct unary factor */ + LinearEquality(Key i1, const Matrix& A1, const Vector& b, Key dualKey) : + Base(i1, A1, b, noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) { + } + + /** Construct binary factor */ + LinearEquality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, + const Vector& b, Key dualKey) : + Base(i1, A1, i2, A2, b, noiseModel::Constrained::All(b.rows())), dualKey_( + dualKey) { + } + + /** Construct ternary factor */ + LinearEquality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, const Vector& b, Key dualKey) : + Base(i1, A1, i2, A2, i3, A3, b, noiseModel::Constrained::All(b.rows())), dualKey_( + dualKey) { + } + + /** Construct four-ary factor */ + LinearEquality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, Key i4, const Matrix& A4, const Vector& b, Key dualKey) : + Base(i1, A1, i2, A2, i3, A3, i4, A4, b, + noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) { + } + + /** Construct five-ary factor */ + LinearEquality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, Key i4, const Matrix& A4, Key i5, const Matrix& A5, + const Vector& b, Key dualKey) : + Base(i1, A1, i2, A2, i3, A3, i4, A4, i5, A5, b, + noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) { + } + + /** Construct six-ary factor */ + LinearEquality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, Key i4, const Matrix& A4, Key i5, const Matrix& A5, + Key i6, const Matrix& A6, const Vector& b, Key dualKey) : + Base(i1, A1, i2, A2, i3, A3, i4, A4, i5, A5, i6, A6, b, + noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) { + } + + /** Construct an n-ary factor + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the factor. */ + template + LinearEquality(const TERMS& terms, const Vector& b, Key dualKey) : + Base(terms, b, noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) { + } + + /** Virtual destructor */ + virtual ~LinearEquality() { + } + + /** equals */ + virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const { + return Base::equals(lf, tol); + } + + /** print */ + virtual void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const { + Base::print(s, formatter); + } + + /** Clone this LinearEquality */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + boost::make_shared(*this)); + } + + /// dual key + Key dualKey() const { return dualKey_; } + + /** Special error_vector for constraints (A*x-b) */ + Vector error_vector(const VectorValues& c) const { + return unweighted_error(c); + } + + /** Special error for constraints. + * I think it should be zero, as this function is meant for objective cost. + * But the name "error" can be misleading. + * TODO: confirm with Frank!! */ + virtual double error(const VectorValues& c) const { + return 0.0; + } + +}; +// LinearEquality + +}// gtsam + diff --git a/gtsam_unstable/linear/LinearEqualityFactorGraph.h b/gtsam_unstable/linear/LinearEqualityFactorGraph.h new file mode 100644 index 000000000..fd9191250 --- /dev/null +++ b/gtsam_unstable/linear/LinearEqualityFactorGraph.h @@ -0,0 +1,32 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * LinearEqualityFactorGraph.h + * @brief: Factor graph of all LinearEquality factors + * @date: Dec 8, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include +#include + +namespace gtsam { + +class LinearEqualityFactorGraph : public FactorGraph { +public: + typedef boost::shared_ptr shared_ptr; +}; + +} // namespace gtsam + diff --git a/gtsam_unstable/linear/LinearInequality.h b/gtsam_unstable/linear/LinearInequality.h new file mode 100644 index 000000000..0edec2ccb --- /dev/null +++ b/gtsam_unstable/linear/LinearInequality.h @@ -0,0 +1,152 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * LinearInequality.h + * @brief: LinearInequality derived from Base with constrained noise model + * @date: Nov 27, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * This class defines Linear constraints by inherit Base + * with the special Constrained noise model + */ +class LinearInequality: public JacobianFactor { +public: + typedef LinearInequality This; ///< Typedef to this class + typedef JacobianFactor Base; ///< Typedef to base class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + +private: + Key dualKey_; + +public: + /** default constructor for I/O */ + LinearInequality() : + Base() { + } + + /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ + explicit LinearInequality(const HessianFactor& hf) { + throw std::runtime_error( + "Cannot convert HessianFactor to LinearInequality"); + } + + /** Construct unary factor */ + LinearInequality(Key i1, const Matrix& A1, const Vector& b, Key dualKey) : + Base(i1, A1, b, noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) { + } + + /** Construct binary factor */ + LinearInequality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, + const Vector& b, Key dualKey) : + Base(i1, A1, i2, A2, b, noiseModel::Constrained::All(b.rows())), dualKey_( + dualKey) { + } + + /** Construct ternary factor */ + LinearInequality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, const Vector& b, Key dualKey) : + Base(i1, A1, i2, A2, i3, A3, b, noiseModel::Constrained::All(b.rows())), dualKey_( + dualKey) { + } + + /** Construct four-ary factor */ + LinearInequality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, Key i4, const Matrix& A4, const Vector& b, Key dualKey) : + Base(i1, A1, i2, A2, i3, A3, i4, A4, b, + noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) { + } + + /** Construct five-ary factor */ + LinearInequality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, Key i4, const Matrix& A4, Key i5, const Matrix& A5, + const Vector& b, Key dualKey) : + Base(i1, A1, i2, A2, i3, A3, i4, A4, i5, A5, b, + noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) { + } + + /** Construct six-ary factor */ + LinearInequality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, Key i4, const Matrix& A4, Key i5, const Matrix& A5, + Key i6, const Matrix& A6, const Vector& b, Key dualKey) : + Base(i1, A1, i2, A2, i3, A3, i4, A4, i5, A5, i6, A6, b, + noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) { + } + + /** Construct an n-ary factor + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the factor. */ + template + LinearInequality(const TERMS& terms, const Vector& b, Key dualKey) : + Base(terms, b, noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) { + } + + /** Virtual destructor */ + virtual ~LinearInequality() { + } + + /** equals */ + virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const { + return Base::equals(lf, tol); + } + + /** print */ + virtual void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const { + Base::print(s, formatter); + } + + /** Clone this LinearInequality */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + boost::make_shared(*this)); + } + + /// dual key + Key dualKey() const { return dualKey_; } + + /** Special error_vector for constraints (A*x-b) */ + Vector error_vector(const VectorValues& c) const { + return unweighted_error(c); + } + + /** Special error for constraints. + * I think it should be zero, as this function is meant for objective cost. + * But the name "error" can be misleading. + * TODO: confirm with Frank!! */ + virtual double error(const VectorValues& c) const { + return 0.0; + } + + /** dot product of row s with the corresponding vector in p */ + double dotProductRow(size_t s, const VectorValues& p) const { + double ajTp = 0.0; + for (const_iterator xj = begin(); xj != end(); ++xj) { + Vector pj = p.at(*xj); + Vector aj = getA(xj).row(s); + ajTp += aj.dot(pj); + } + return ajTp; + } + +}; +// LinearInequality + +}// gtsam + diff --git a/gtsam_unstable/linear/LinearInequalityFactorGraph.h b/gtsam_unstable/linear/LinearInequalityFactorGraph.h new file mode 100644 index 000000000..bfc64df2a --- /dev/null +++ b/gtsam_unstable/linear/LinearInequalityFactorGraph.h @@ -0,0 +1,48 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * LinearInequalityFactorGraph.h + * @brief: Factor graph of all LinearInequality factors + * @date: Dec 8, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +class LinearInequalityFactorGraph: public FactorGraph { +private: + typedef FactorGraph Base; + +public: + typedef boost::shared_ptr shared_ptr; + + /** print */ + void print(const std::string& str, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + Base::print(str, keyFormatter); + } + + /** equals */ + bool equals(const LinearInequalityFactorGraph& other, + double tol = 1e-9) const { + return Base::equals(other, tol); + } +}; + +} // namespace gtsam + diff --git a/gtsam_unstable/linear/QP.h b/gtsam_unstable/linear/QP.h new file mode 100644 index 000000000..111ab506f --- /dev/null +++ b/gtsam_unstable/linear/QP.h @@ -0,0 +1,58 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * QP.h + * @brief: Factor graphs of a Quadratic Programming problem + * @date: Dec 8, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * struct contains factor graphs of a Quadratic Programming problem + */ +struct QP { + GaussianFactorGraph cost; //!< Quadratic cost factors + LinearEqualityFactorGraph equalities; //!< linear equality constraints + LinearInequalityFactorGraph inequalities; //!< linear inequality constraints + + /** default constructor */ + QP() : + cost(), equalities(), inequalities() { + } + + /** constructor */ + QP(const GaussianFactorGraph& _cost, + const LinearEqualityFactorGraph& _linearEqualities, + const LinearInequalityFactorGraph& _linearInequalities) : + cost(_cost), equalities(_linearEqualities), inequalities( + _linearInequalities) { + } + + /** print */ + void print(const std::string& s = "") { + std::cout << s << std::endl; + cost.print("Quadratic cost factors: "); + equalities.print("Linear equality factors: "); + inequalities.print("Linear inequality factors: "); + } +}; + +} // namespace gtsam + diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index ac6bc549c..7872ce27e 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -6,255 +6,85 @@ */ #include +#include #include #include -#include #include using namespace std; +#define ACTIVE 0.0 +#define INACTIVE std::numeric_limits::infinity() + namespace gtsam { -/// Convert a Gaussian factor to a jacobian. return empty shared ptr if failed -static JacobianFactor::shared_ptr toJacobian( - const GaussianFactor::shared_ptr& factor) { - JacobianFactor::shared_ptr jacobian( - boost::dynamic_pointer_cast(factor)); - return jacobian; +//****************************************************************************** +QPSolver::QPSolver(const QP& qp) : qp_(qp) { + baseGraph_ = qp_.cost; + baseGraph_.push_back(qp_.equalities.begin(), qp_.equalities.end()); + costVariableIndex_ = VariableIndex(qp_.cost); + equalityVariableIndex_ = VariableIndex(qp_.equalities); + inequalityVariableIndex_ = VariableIndex(qp_.inequalities); + constrainedKeys_ = qp_.equalities.keys(); + constrainedKeys_.merge(qp_.inequalities.keys()); } //****************************************************************************** -QPSolver::QPSolver(const GaussianFactorGraph& graph) : - graph_(graph), fullFactorIndices_(graph) { - // Split the original graph into unconstrained and constrained part - // and collect indices of constrained factors - for (size_t i = 0; i < graph.nrFactors(); ++i) { - // obtain the factor and its noise model - JacobianFactor::shared_ptr jacobian = toJacobian(graph.at(i)); - if (jacobian && jacobian->get_model() - && jacobian->get_model()->isConstrained()) { - constraintIndices_.push_back(i); - } - } - - // Collect constrained variable keys - set constrainedVars; - BOOST_FOREACH(size_t index, constraintIndices_) { - KeyVector keys = graph.at(index)->keys(); - constrainedVars.insert(keys.begin(), keys.end()); - } - - // Collect unconstrained hessians of constrained vars to build dual graph - findUnconstrainedHessiansOfConstrainedVars(constrainedVars); - freeHessianFactorIndex_ = VariableIndex(freeHessians_); +VectorValues QPSolver::solveWithCurrentWorkingSet( + const LinearInequalityFactorGraph& workingSet) const { + GaussianFactorGraph workingGraph = baseGraph_; + workingGraph.push_back(workingSet); + return workingGraph.optimize(); } //****************************************************************************** -void QPSolver::findUnconstrainedHessiansOfConstrainedVars( - const set& constrainedVars) { - VariableIndex variableIndex(graph_); +JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key, + const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const { - // Collect all factors involving constrained vars - FastSet factors; - BOOST_FOREACH(Key key, constrainedVars) { - VariableIndex::Factors factorsOfThisVar = variableIndex[key]; - BOOST_FOREACH(size_t factorIndex, factorsOfThisVar) { - factors.insert(factorIndex); - } - } + // Transpose the A matrix of constrained factors to have the jacobian of the dual key + std::vector > Aterms = collectDualJacobians + < LinearEquality > (key, qp_.equalities, equalityVariableIndex_); + std::vector > AtermsInequalities = collectDualJacobians + < LinearInequality > (key, workingSet, inequalityVariableIndex_); + Aterms.insert(Aterms.end(), AtermsInequalities.begin(), + AtermsInequalities.end()); - // Convert each factor into Hessian - BOOST_FOREACH(size_t factorIndex, factors) { - GaussianFactor::shared_ptr gf = graph_[factorIndex]; - if (!gf) - continue; - // See if this is a Jacobian factor - JacobianFactor::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - if (jf) { - // Dealing with mixed constrained factor - if (jf->get_model() && jf->isConstrained()) { - // Turn a mixed-constrained factor into a factor with 0 information on the constrained part - Vector sigmas = jf->get_model()->sigmas(); - Vector newPrecisions(sigmas.size()); - bool mixed = false; - for (size_t s = 0; s < sigmas.size(); ++s) { - if (sigmas[s] <= 1e-9) - newPrecisions[s] = 0.0; // 0 info for constraints (both inequality and eq) - else { - newPrecisions[s] = 1.0 / sigmas[s]; - mixed = true; - } - } - if (mixed) { // only add free hessians if it's mixed - JacobianFactor::shared_ptr newJacobian = toJacobian(jf->clone()); - newJacobian->setModel( - noiseModel::Diagonal::Precisions(newPrecisions)); - freeHessians_.push_back(HessianFactor(*newJacobian)); - } - } else { // unconstrained Jacobian - // Convert the original linear factor to Hessian factor - // TODO: This may fail and throw the following exception - // Assertion failed: (((!PanelMode) && stride==0 && offset==0) || - // (PanelMode && stride>=depth && offset<=stride)), function operator(), - // file Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h, line 1133. - // because of a weird error which might be related to clang - // See this: https://groups.google.com/forum/#!topic/ceres-solver/DYhqOLPquHU - // My current way to fix this is to compile both gtsam and my library in Release mode - freeHessians_.add(HessianFactor(*jf)); - } - } else { // If it's not a Jacobian, it should be a hessian factor. Just add! - HessianFactor::shared_ptr hf = // - boost::dynamic_pointer_cast(gf); - if (hf) - freeHessians_.push_back(hf); - } + // Collect the gradients of unconstrained cost factors to the b vector + Vector b = zero(delta.at(key).size()); + BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) { + GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); + b += factor->gradient(key, delta); } + return boost::make_shared(Aterms, b); } //****************************************************************************** -GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, - const VectorValues& x0, bool useLeastSquare) const { - static const bool debug = false; - - // The dual graph to return - GaussianFactorGraph dualGraph; - - // For each variable xi involving in some constraint, compute the unconstrained gradient - // wrt xi from the prebuilt freeHessian graph - // \grad f(xi) = \frac{\partial f}{\partial xi}' = \sum_j G_ij*xj - gi - if (debug) - freeHessianFactorIndex_.print("freeHessianFactorIndex_: "); - BOOST_FOREACH(const VariableIndex::value_type& xiKey_factors, freeHessianFactorIndex_) { - Key xiKey = xiKey_factors.first; - VariableIndex::Factors xiFactors = xiKey_factors.second; - - // Find xi's dim from the first factor on xi - if (xiFactors.size() == 0) - continue; - GaussianFactor::shared_ptr xiFactor0 = freeHessians_.at(*xiFactors.begin()); - size_t xiDim = xiFactor0->getDim(xiFactor0->find(xiKey)); - if (debug) - xiFactor0->print("xiFactor0: "); - if (debug) - cout << "xiKey: " << string(Symbol(xiKey)) << ", xiDim: " << xiDim - << endl; - - //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - // Compute the b-vector for the dual factor Ax-b - // b = gradf(xi) = \frac{\partial f}{\partial xi}' = \sum_j G_ij*xj - gi - Vector gradf_xi = zero(xiDim); - BOOST_FOREACH(size_t factorIx, xiFactors) { - HessianFactor::shared_ptr factor = freeHessians_.at(factorIx); - Factor::const_iterator xi = factor->find(xiKey); - // Sum over Gij*xj for all xj connecting to xi - for (Factor::const_iterator xj = factor->begin(); xj != factor->end(); - ++xj) { - // Obtain Gij from the Hessian factor - // Hessian factor only stores an upper triangular matrix, so be careful when i>j - Matrix Gij; - if (xi > xj) { - Matrix Gji = factor->info(xj, xi); - Gij = Gji.transpose(); - } else { - Gij = factor->info(xi, xj); - } - // Accumulate Gij*xj to gradf - Vector x0_j = x0.at(*xj); - gradf_xi += Gij * x0_j; - } - // Subtract the linear term gi - gradf_xi += -factor->linearTerm(xi); - } - - //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - // Compute the Jacobian A for the dual factor Ax-b - // Obtain the jacobians for lambda variables from their corresponding constraints - // A = gradc_k(xi) = \frac{\partial c_k}{\partial xi}' - vector > lambdaTerms; // collection of lambda_k, and gradc_k - typedef pair FactorIx_SigmaIx; - vector unconstrainedIndex; // pairs of factorIx,sigmaIx of unconstrained rows - BOOST_FOREACH(size_t factorIndex, fullFactorIndices_[xiKey]) { - JacobianFactor::shared_ptr factor = toJacobian(graph.at(factorIndex)); - if (!factor || !factor->isConstrained()) - continue; - // Gradient is the transpose of the Jacobian: A_k = gradc_k(xi) = \frac{\partial c_k}{\partial xi}' - // Each column for each lambda_k corresponds to [the transpose of] each constrained row factor - Matrix A_k = factor->getA(factor->find(xiKey)).transpose(); - if (debug) - gtsam::print(A_k, "A_k = "); - - // Deal with mixed sigmas: no information if sigma != 0 - Vector sigmas = factor->get_model()->sigmas(); - for (size_t sigmaIx = 0; sigmaIx < sigmas.size(); ++sigmaIx) { - // if it's either inequality (sigma<0) or unconstrained (sigma>0) - // we have no information about it - if (fabs(sigmas[sigmaIx]) > 1e-9) { - A_k.col(sigmaIx) = zero(A_k.rows()); - // remember to add a zero prior on this lambda, otherwise the graph is under-determined - unconstrainedIndex.push_back(make_pair(factorIndex, sigmaIx)); - } - } - - // Use factorIndex as the lambda's key. - lambdaTerms.push_back(make_pair(factorIndex, A_k)); - } - - //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - // Create and add factors to the dual graph - // If least square approximation is desired, use unit noise model. - if (debug) - cout << "Create dual factor" << endl; - if (useLeastSquare) { - if (debug) - cout << "use least square!" << endl; - dualGraph.push_back( - JacobianFactor(lambdaTerms, gradf_xi, - noiseModel::Unit::Create(gradf_xi.size()))); - } else { - // Enforce constrained noise model so lambdas are solved with QR - // and should exactly satisfy all the equations - if (debug) - cout << gradf_xi << endl; - dualGraph.push_back( - JacobianFactor(lambdaTerms, gradf_xi, - noiseModel::Constrained::All(gradf_xi.size()))); - } - - // Add 0 priors on all lambdas of the unconstrained rows to make sure the graph is solvable - if (debug) - cout << "Create priors" << endl; - BOOST_FOREACH(FactorIx_SigmaIx factorIx_sigmaIx, unconstrainedIndex) { - size_t factorIx = factorIx_sigmaIx.first; - JacobianFactor::shared_ptr factor = toJacobian(graph.at(factorIx)); - size_t dim = factor->get_model()->dim(); - Matrix J = zeros(dim, dim); - size_t sigmaIx = factorIx_sigmaIx.second; - J(sigmaIx, sigmaIx) = 1.0; - // Use factorIndex as the lambda's key. - if (debug) - cout << "prior for factor " << factorIx << endl; - dualGraph.push_back(JacobianFactor(factorIx, J, zero(dim))); - } +GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph( + const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const { + GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); + BOOST_FOREACH(Key key, constrainedKeys_) { + // Each constrained key becomes a factor in the dual graph + dualGraph->push_back(createDualFactor(key, workingSet, delta)); } - return dualGraph; } //****************************************************************************** pair QPSolver::identifyLeavingConstraint( + const LinearInequalityFactorGraph& workingSet, const VectorValues& lambdas) const { int worstFactorIx = -1, worstSigmaIx = -1; // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either // inactive or a good inequality constraint, so we don't care! double maxLambda = 0.0; - BOOST_FOREACH(size_t factorIx, constraintIndices_) { - Vector lambda = lambdas.at(factorIx); - Vector orgSigmas = toJacobian(graph_.at(factorIx))->get_model()->sigmas(); - for (size_t j = 0; j < orgSigmas.size(); ++j) - // If it is a BAD active inequality, and lambda is larger than the current max - if (orgSigmas[j] < 0 && lambda[j] > maxLambda) { + for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { + const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); + Vector lambda = lambdas.at(factor->dualKey()); + Vector sigmas = factor->get_model()->sigmas(); + for (size_t j = 0; j < sigmas.size(); ++j) + // If it is an active constraint, and lambda is larger than the current max + if (sigmas[j] == ACTIVE && lambda[j] > maxLambda) { worstFactorIx = factorIx; worstSigmaIx = j; maxLambda = lambda[j]; @@ -264,14 +94,16 @@ pair QPSolver::identifyLeavingConstraint( } //****************************************************************************** -bool QPSolver::updateWorkingSetInplace(GaussianFactorGraph& workingGraph, - int factorIx, int sigmaIx, double newSigma) const { +LinearInequalityFactorGraph QPSolver::updateWorkingSet( + const LinearInequalityFactorGraph& workingSet, int factorIx, int sigmaIx, + double state) const { + LinearInequalityFactorGraph newWorkingSet = workingSet; if (factorIx < 0 || sigmaIx < 0) - return false; - Vector sigmas = toJacobian(workingGraph.at(factorIx))->get_model()->sigmas(); - sigmas[sigmaIx] = newSigma; // removing it from the working set - toJacobian(workingGraph.at(factorIx))->setModel(true, sigmas); - return true; + return newWorkingSet; + Vector sigmas = newWorkingSet.at(factorIx)->get_model()->sigmas(); + sigmas[sigmaIx] = state; + newWorkingSet.at(factorIx)->setModel(true, sigmas); + return newWorkingSet; } //****************************************************************************** @@ -291,44 +123,28 @@ bool QPSolver::updateWorkingSetInplace(GaussianFactorGraph& workingGraph, * We want the minimum of all those alphas among all inactive inequality. */ boost::tuple QPSolver::computeStepSize( - const GaussianFactorGraph& workingGraph, const VectorValues& xk, + const LinearInequalityFactorGraph& workingSet, const VectorValues& xk, const VectorValues& p) const { static bool debug = false; double minAlpha = 1.0; int closestFactorIx = -1, closestSigmaIx = -1; - BOOST_FOREACH(size_t factorIx, constraintIndices_) { - JacobianFactor::shared_ptr jacobian = toJacobian(workingGraph.at(factorIx)); - Vector sigmas = jacobian->get_model()->sigmas(); - Vector b = jacobian->getb(); + for(size_t factorIx = 0; factorIxget_model()->sigmas(); + Vector b = factor->getb(); for (size_t s = 0; s < sigmas.size(); ++s) { // If it is an inactive inequality, compute alpha and update min - if (sigmas[s] < 0) { + if (sigmas[s] == INACTIVE) { // Compute aj'*p - double ajTp = 0.0; - for (Factor::const_iterator xj = jacobian->begin(); - xj != jacobian->end(); ++xj) { - Vector pj = p.at(*xj); - Vector aj = jacobian->getA(xj).row(s); - ajTp += aj.dot(pj); - } - if (debug) - cout << "s, ajTp, b[s]: " << s << " " << ajTp << " " << b[s] << endl; + double ajTp = factor->dotProductRow(s, p); // Check if aj'*p >0. Don't care if it's not. if (ajTp <= 0) continue; // Compute aj'*xk - double ajTx = 0.0; - for (Factor::const_iterator xj = jacobian->begin(); - xj != jacobian->end(); ++xj) { - Vector xkj = xk.at(*xj); - Vector aj = jacobian->getA(xj).row(s); - ajTx += aj.dot(xkj); - } - if (debug) - cout << "b[s], ajTx: " << b[s] << " " << ajTx << " " << ajTp << endl; + double ajTx = factor->dotProductRow(s, xk); // alpha = (bj - aj'*xk) / (aj'*p) double alpha = (b[s] - ajTx) / ajTp; @@ -348,171 +164,224 @@ boost::tuple QPSolver::computeStepSize( } //****************************************************************************** -bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, - VectorValues& currentSolution, VectorValues& lambdas) const { +QPState QPSolver::iterate(const QPState& state) const { static bool debug = false; + + // Solve with the current working set + VectorValues newValues = solveWithCurrentWorkingSet(state.workingSet); if (debug) - workingGraph.print("workingGraph: "); - // Obtain the solution from the current working graph - VectorValues newSolution = workingGraph.optimize(); - if (debug) - newSolution.print("New solution:"); + newValues.print("New solution:"); // If we CAN'T move further - if (newSolution.equals(currentSolution, 1e-5)) { + if (newValues.equals(state.values, 1e-5)) { // Compute lambda from the dual graph if (debug) cout << "Building dual graph..." << endl; - GaussianFactorGraph dualGraph = buildDualGraph(workingGraph, newSolution); + GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, newValues); if (debug) - dualGraph.print("Dual graph: "); - lambdas = dualGraph.optimize(); + dualGraph->print("Dual graph: "); + VectorValues duals = dualGraph->optimize(); if (debug) - lambdas.print("lambdas :"); + duals.print("Duals :"); - int factorIx, sigmaIx; - boost::tie(factorIx, sigmaIx) = identifyLeavingConstraint(lambdas); + int leavingFactor, leavingSigmaIx; + boost::tie(leavingFactor, leavingSigmaIx) = // + identifyLeavingConstraint(state.workingSet, duals); if (debug) - cout << "violated active inequality - factorIx, sigmaIx: " << factorIx - << " " << sigmaIx << endl; + cout << "violated active inequality - factorIx, sigmaIx: " << leavingFactor + << " " << leavingSigmaIx << endl; - // Try to de-activate the weakest violated inequality constraints - // if not successful, i.e. all inequality constraints are satisfied: - // We have the solution!! - if (!updateWorkingSetInplace(workingGraph, factorIx, sigmaIx, -1.0)) - return true; - } else { + // If all inequality constraints are satisfied: We have the solution!! + if (leavingFactor < 0 || leavingSigmaIx < 0) { + return QPState(newValues, duals, state.workingSet, true); + } + else { + // Inactivate the leaving constraint + LinearInequalityFactorGraph newWorkingSet = updateWorkingSet( + state.workingSet, leavingFactor, leavingSigmaIx, INACTIVE); + return QPState(newValues, duals, newWorkingSet, false); + } + } + else { // If we CAN make some progress // Adapt stepsize if some inactive constraints complain about this move - if (debug) - cout << "Computing stepsize..." << endl; double alpha; int factorIx, sigmaIx; - VectorValues p = newSolution - currentSolution; + VectorValues p = newValues - state.values; boost::tie(alpha, factorIx, sigmaIx) = // - computeStepSize(workingGraph, currentSolution, p); + computeStepSize(state.workingSet, state.values, p); if (debug) cout << "alpha, factorIx, sigmaIx: " << alpha << " " << factorIx << " " << sigmaIx << endl; // also add to the working set the one that complains the most - updateWorkingSetInplace(workingGraph, factorIx, sigmaIx, 0.0); - // step! - currentSolution = currentSolution + alpha * p; -// if (alpha <1e-5) { -// if (debug) cout << "Building dual graph..." << endl; -// GaussianFactorGraph dualGraph = buildDualGraph(workingGraph, newSolution); -// if (debug) dualGraph.print("Dual graph: "); -// lambdas = dualGraph.optimize(); -// if (debug) lambdas.print("lambdas :"); -// return true; // TODO: HACK HACK!!! -// } - } + LinearInequalityFactorGraph newWorkingSet = // + updateWorkingSet(state.workingSet, factorIx, sigmaIx, ACTIVE); - return false; + // step! + newValues = state.values + alpha * p; + + return QPState(newValues, state.duals, newWorkingSet, false); + } } //****************************************************************************** pair QPSolver::optimize( const VectorValues& initialValues) const { - GaussianFactorGraph workingGraph = graph_.clone(); - VectorValues currentSolution = initialValues; - VectorValues lambdas; - bool converged = false; - while (!converged) { - converged = iterateInPlace(workingGraph, currentSolution, lambdas); + + // TODO: initialize workingSet from the feasible initialValues + LinearInequalityFactorGraph workingSet(qp_.inequalities); + + QPState state(initialValues, VectorValues(), workingSet, false); + + /// main loop of the solver + while (!state.converged) { + state = iterate(state); } - return make_pair(currentSolution, lambdas); + + return make_pair(state.values, state.duals); } //****************************************************************************** -pair QPSolver::initialValuesLP() const { - size_t firstSlackKey = 0; - BOOST_FOREACH(Key key, fullFactorIndices_ | boost::adaptors::map_keys) { - if (firstSlackKey < key) - firstSlackKey = key; - } +std::pair QPSolver::maxKey(const FastSet& keys) const { + KeySet::iterator maxEl = std::max_element(keys.begin(), keys.end()); + if (maxEl==keys.end()) + return make_pair(false, 0); + return make_pair(true, *maxEl); +} + +//****************************************************************************** +boost::tuple QPSolver::initialValuesLP() const { + // Key for the first slack variable = maximum key + 1 + size_t firstSlackKey; + bool found; + KeySet allKeys = qp_.cost.keys(); + allKeys.merge(qp_.equalities.keys()); + allKeys.merge(qp_.inequalities.keys()); + boost::tie(found, firstSlackKey) = maxKey(allKeys); firstSlackKey += 1; 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(const LinearEquality::shared_ptr& factor, qp_.equalities) { + KeyVector keys = factor->keys(); BOOST_FOREACH(Key key, keys) { if (!initialValues.exists(key)) { - size_t dim = jacobian->getDim(jacobian->find(key)); + size_t dim = factor->getDim(factor->find(key)); + initialValues.insert(key, zero(dim)); + } + } + } + + BOOST_FOREACH(const LinearInequality::shared_ptr& factor, qp_.inequalities) { + KeyVector keys = factor->keys(); + BOOST_FOREACH(Key key, keys) { + if (!initialValues.exists(key)) { + size_t dim = factor->getDim(factor->find(key)); initialValues.insert(key, zero(dim)); } } } // Insert initial values for slack variables - size_t slackKey = firstSlackKey; - BOOST_FOREACH(size_t iFactor, constraintIndices_) { - JacobianFactor::shared_ptr jacobian = toJacobian(graph_.at(iFactor)); - Vector errorAtZero = jacobian->getb(); - Vector slackInit = zero(errorAtZero.size()); - Vector sigmas = jacobian->get_model()->sigmas(); - for (size_t i = 0; i < sigmas.size(); ++i) { - if (sigmas[i] < 0) { - slackInit[i] = std::max(errorAtZero[i], 0.0); - } else if (sigmas[i] == 0.0) { - errorAtZero[i] = fabs(errorAtZero[i]); - } // if it has >0 sigma, i.e. normal Gaussian noise, initialize it at 0 - } + Key slackKey = firstSlackKey; + // Equality: zi = |bi| + BOOST_FOREACH(const LinearEquality::shared_ptr& factor, qp_.equalities) { + Vector errorAtZero = factor->getb(); + Vector slackInit = errorAtZero.cwiseAbs(); initialValues.insert(slackKey, slackInit); slackKey++; } - return make_pair(initialValues, firstSlackKey); + // Inequality: zi = max(bi, 0) + BOOST_FOREACH(const LinearInequality::shared_ptr& factor, qp_.inequalities) { + Vector errorAtZero = factor->getb(); + Vector zeroVec = zero(errorAtZero.size()); + Vector slackInit = errorAtZero.cwiseMax(zeroVec); + initialValues.insert(slackKey, slackInit); + slackKey++; + } + + return boost::make_tuple(initialValues, firstSlackKey, slackKey - 1); } //****************************************************************************** VectorValues QPSolver::objectiveCoeffsLP(Key firstSlackKey) const { VectorValues slackObjective; - for (size_t i = 0; i < constraintIndices_.size(); ++i) { - Key key = firstSlackKey + i; - size_t iFactor = constraintIndices_[i]; - JacobianFactor::shared_ptr jacobian = toJacobian(graph_.at(iFactor)); - size_t dim = jacobian->rows(); - Vector objective = ones(dim); - /* We should not ignore unconstrained slack var dimensions (those rows with sigmas >0) - * because their values might be underdetermined in the LP. Since they will have only - * 1 constraint zi>=0, enforcing them in the min obj function won't harm the other constrained - * slack vars, but also makes them well defined: 0 at the minimum. - */ - slackObjective.insert(key, ones(dim)); + + Key slackKey = firstSlackKey; + // Equalities + BOOST_FOREACH(const LinearEquality::shared_ptr& factor, qp_.equalities) { + size_t dim = factor->rows(); + slackObjective.insert(slackKey, ones(dim)); + slackKey++; } + + // Inequalities + BOOST_FOREACH(const LinearInequality::shared_ptr& factor, qp_.inequalities) { + size_t dim = factor->rows(); + slackObjective.insert(slackKey, ones(dim)); + slackKey++; + } + return slackObjective; } //****************************************************************************** -pair QPSolver::constraintsLP( +boost::tuple QPSolver::constraintsLP( Key firstSlackKey) const { - // Create constraints and 0 lower bounds (zi>=0) - GaussianFactorGraph::shared_ptr constraints(new GaussianFactorGraph()); + // Create constraints and zero lower bounds (zi>=0) + LinearEqualityFactorGraph::shared_ptr equalities(new LinearEqualityFactorGraph()); + LinearInequalityFactorGraph::shared_ptr inequalities(new LinearInequalityFactorGraph()); VectorValues slackLowerBounds; - for (size_t key = firstSlackKey; - key < firstSlackKey + constraintIndices_.size(); ++key) { - size_t iFactor = constraintIndices_[key - firstSlackKey]; - JacobianFactor::shared_ptr jacobian = toJacobian(graph_.at(iFactor)); + + Key slackKey = firstSlackKey; + + // Equalities + BOOST_FOREACH(const LinearEquality::shared_ptr& factor, qp_.equalities) { // Collect old terms to form a new factor // TODO: it might be faster if we can get the whole block matrix at once // but I don't know how to extend the current VerticalBlockMatrix vector > terms; - for (Factor::iterator it = jacobian->begin(); it != jacobian->end(); ++it) { - terms.push_back(make_pair(*it, jacobian->getA(it))); + for (Factor::iterator it = factor->begin(); it != factor->end(); ++it) { + terms.push_back(make_pair(*it, factor->getA(it))); } + + Vector b = factor->getb(); + Vector sign_b = b.cwiseQuotient(b.cwiseAbs()); + terms.push_back(make_pair(slackKey, sign_b)); + equalities->push_back(LinearEquality(terms, b, factor->dualKey())); + + // Add lower bound for this slack key + slackLowerBounds.insert(slackKey, zero(b.rows())); + // Increase slackKey for the next slack variable + slackKey++; + } + + // Inequalities + BOOST_FOREACH(const LinearInequality::shared_ptr& factor, qp_.inequalities) { + // Collect old terms to form a new factor + // TODO: it might be faster if we can get the whole block matrix at once + // but I don't know how to extend the current VerticalBlockMatrix + vector > terms; + for (Factor::iterator it = factor->begin(); it != factor->end(); ++it) { + terms.push_back(make_pair(*it, factor->getA(it))); + } + // Add the slack term to the constraint // Unlike Nocedal06book, pg.473, we want ax-z <= b, since we always assume - // LE constraints ax <= b for sigma < 0. - size_t dim = jacobian->rows(); - terms.push_back(make_pair(key, -eye(dim))); - constraints->push_back( - JacobianFactor(terms, jacobian->getb(), jacobian->get_model())); + // LE constraints ax <= b. + size_t dim = factor->rows(); + terms.push_back(make_pair(slackKey, -eye(dim))); + inequalities->push_back(LinearInequality(terms, factor->getb(), + factor->dualKey())); + // Add lower bound for this slack key - slackLowerBounds.insert(key, zero(dim)); + slackLowerBounds.insert(slackKey, zero(dim)); + // Increase slackKey for the next slack variable + slackKey++; } - return make_pair(constraints, slackLowerBounds); + + return boost::make_tuple(equalities, inequalities, slackLowerBounds); } //****************************************************************************** @@ -520,19 +389,20 @@ pair QPSolver::findFeasibleInitialValues() const { static const bool debug = false; // Initial values with slack variables for the LP subproblem, Nocedal06book, pg.473 VectorValues initialValues; - size_t firstSlackKey; - boost::tie(initialValues, firstSlackKey) = initialValuesLP(); + size_t firstSlackKey, lastSlackKey; + boost::tie(initialValues, firstSlackKey, lastSlackKey) = initialValuesLP(); // Coefficients for the LP subproblem objective function, min \sum_i z_i VectorValues objectiveLP = objectiveCoeffsLP(firstSlackKey); // Create constraints and lower bounds of slack variables - GaussianFactorGraph::shared_ptr constraints; + LinearEqualityFactorGraph::shared_ptr equalities; + LinearInequalityFactorGraph::shared_ptr inequalities; VectorValues slackLowerBounds; - boost::tie(constraints, slackLowerBounds) = constraintsLP(firstSlackKey); + boost::tie(equalities, inequalities, slackLowerBounds) = constraintsLP(firstSlackKey); // Solve the LP subproblem - LPSolver lpSolver(objectiveLP, constraints, slackLowerBounds); + LPSolver lpSolver(objectiveLP, equalities, inequalities, slackLowerBounds); VectorValues solution = lpSolver.solve(); if (debug) @@ -540,29 +410,34 @@ pair QPSolver::findFeasibleInitialValues() const { if (debug) objectiveLP.print("Objective LP: "); if (debug) - constraints->print("Constraints LP: "); + equalities->print("Equalities LP: "); + if (debug) + inequalities->print("Inequalities LP: "); if (debug) solution.print("LP solution: "); + // feasible when all slack values are 0s. + double slackSumAbs = 0.0; + for (Key key = firstSlackKey; key <= lastSlackKey; ++key) { + slackSumAbs += solution.at(key).cwiseAbs().sum(); + } + // Remove slack variables from solution - double slackSum = 0.0; - for (Key key = firstSlackKey; key < firstSlackKey + constraintIndices_.size(); - ++key) { - slackSum += solution.at(key).cwiseAbs().sum(); + for (Key key = firstSlackKey; key <= lastSlackKey; ++key) { solution.erase(key); } // Insert zero vectors for free variables that are not in the constraints - BOOST_FOREACH(Key key, fullFactorIndices_ | boost::adaptors::map_keys) { + BOOST_FOREACH(Key key, costVariableIndex_ | boost::adaptors::map_keys) { if (!solution.exists(key)) { - GaussianFactor::shared_ptr factor = graph_.at( - *fullFactorIndices_[key].begin()); + GaussianFactor::shared_ptr factor = qp_.cost.at( + *costVariableIndex_[key].begin()); size_t dim = factor->getDim(factor->find(key)); solution.insert(key, zero(dim)); } } - return make_pair(slackSum < 1e-5, solution); + return make_pair(slackSumAbs < 1e-5, solution); } //****************************************************************************** diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index ea4c18fa9..b3c66a21f 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -7,14 +7,34 @@ #pragma once -#include #include +#include #include #include namespace gtsam { +/// This struct holds the state of QPSolver at each iteration +struct QPState { + VectorValues values; + VectorValues duals; + LinearInequalityFactorGraph workingSet; + bool converged; + + /// default constructor + QPState() : + values(), duals(), workingSet(), converged(false) { + } + + /// constructor with initial values + QPState(const VectorValues& initialValues, const VectorValues& initialDuals, + const LinearInequalityFactorGraph& initialWorkingSet, bool _converged) : + values(initialValues), duals(initialDuals), workingSet(initialWorkingSet), converged( + _converged) { + } +}; + /** * This class implements the active set method to solve quadratic programming problems * encoded in a GaussianFactorGraph with special mixed constrained noise models, in which @@ -24,32 +44,41 @@ namespace gtsam { */ class QPSolver { - class Hessians: public FactorGraph { - }; - - const GaussianFactorGraph& graph_; //!< the original graph, can't be modified! - FastVector constraintIndices_; //!< Indices of constrained factors in the original graph - Hessians freeHessians_; //!< unconstrained Hessians of constrained variables - VariableIndex freeHessianFactorIndex_; //!< indices of unconstrained Hessian factors of constrained variables - // gtsam calls it "VariableIndex", but I think FactorIndex - // makes more sense, because it really stores factor indices. - VariableIndex fullFactorIndices_; //!< indices of factors involving each variable. - // gtsam calls it "VariableIndex", but I think FactorIndex - // makes more sense, because it really stores factor indices. + const QP& qp_; //!< factor graphs of the QP problem, can't be modified! + GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. The working set of inequalities will be added to this base graph in the process. + VariableIndex costVariableIndex_, equalityVariableIndex_, + inequalityVariableIndex_; + FastSet constrainedKeys_; //!< all constrained keys, will become factors in the dual graph public: /// Constructor - QPSolver(const GaussianFactorGraph& graph); + QPSolver(const QP& qp); - /// Return indices of all constrained factors - FastVector constraintIndices() const { - return constraintIndices_; + /// Find solution with the current working set + VectorValues solveWithCurrentWorkingSet( + const LinearInequalityFactorGraph& workingSet) const; + + /// @name Build the dual graph + /// @{ + + /// Collect the Jacobian terms for a dual factor + template + std::vector > collectDualJacobians(Key key, + const FactorGraph& graph, + const VariableIndex& variableIndex) const { + std::vector > Aterms; + BOOST_FOREACH(size_t factorIx, variableIndex[key]){ + typename FACTOR::shared_ptr factor = graph.at(factorIx); + Matrix Ai = factor->getA(factor->find(key)).transpose(); + Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); + } + return Aterms; } - /// Return the Hessian factor graph of constrained variables - const Hessians& freeHessiansOfConstrainedVars() const { - return freeHessians_; - } + /// Create a dual factor + JacobianFactor::shared_ptr createDualFactor(Key key, + const LinearInequalityFactorGraph& workingSet, + const VectorValues& delta) const; /** * Build the dual graph to solve for the Lagrange multipliers. @@ -78,8 +107,11 @@ public: * - The constant term b is \grad f(xi), which can be computed from all unconstrained * Hessian factors connecting to xi: \grad f(xi) = \sum_j G_ij*xj - gi */ - GaussianFactorGraph buildDualGraph(const GaussianFactorGraph& graph, - const VectorValues& x0, bool useLeastSquare = false) const; + GaussianFactorGraph::shared_ptr buildDualGraph( + const LinearInequalityFactorGraph& workingSet, + const VectorValues& delta) const; + + /// @} /** * The goal of this function is to find currently active inequality constraints @@ -116,15 +148,15 @@ public: * */ std::pair identifyLeavingConstraint( + const LinearInequalityFactorGraph& workingSet, const VectorValues& lambdas) const; /** - * Deactivate or activate an inequality constraint in place - * Warning: modify in-place to avoid copy/clone - * @return true if update successful + * Inactivate or activate an inequality constraint */ - bool updateWorkingSetInplace(GaussianFactorGraph& workingGraph, int factorIx, - int sigmaIx, double newSigma) const; + LinearInequalityFactorGraph updateWorkingSet( + const LinearInequalityFactorGraph& workingSet, int factorIx, int sigmaIx, + double state) const; /** * Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1] @@ -135,12 +167,11 @@ public: * in the next iteration */ boost::tuple computeStepSize( - const GaussianFactorGraph& workingGraph, const VectorValues& xk, + const LinearInequalityFactorGraph& workingSet, const VectorValues& xk, const VectorValues& p) const; - /** Iterate 1 step, modify workingGraph and currentSolution *IN PLACE* !!! */ - bool iterateInPlace(GaussianFactorGraph& workingGraph, - VectorValues& currentSolution, VectorValues& lambdas) const; + /** Iterate 1 step, return a new state with a new workingSet and values */ + QPState iterate(const QPState& state) const; /** Optimize with a provided initial values * For this version, it is the responsibility of the caller to provide @@ -161,27 +192,27 @@ public: std::pair optimize() const; /** - * Create initial values for the LP subproblem - * @return initial values and the key for the first slack variable + * find the max key */ - std::pair initialValuesLP() const; + std::pair maxKey(const FastSet& keys) const; + + /** + * Create initial values for the LP subproblem + * @return initial values and the key for the first and last slack variables + */ + boost::tuple initialValuesLP() const; /// Create coefficients for the LP subproblem's objective function as the sum of slack var VectorValues objectiveCoeffsLP(Key firstSlackKey) const; /// Build constraints and slacks' lower bounds for the LP subproblem - std::pair constraintsLP( + boost::tuple constraintsLP( Key firstSlackKey) const; /// Find a feasible initial point std::pair findFeasibleInitialValues() const; -private: - - /// Collect all free Hessians involving constrained variables into a graph - void findUnconstrainedHessiansOfConstrainedVars( - const std::set& constrainedVars); - }; } /* namespace gtsam */ diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 6d8e07901..edf64655d 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -31,14 +31,14 @@ using namespace gtsam::symbol_shorthand; /* ************************************************************************* */ // Create test graph according to Forst10book_pg171Ex5 -GaussianFactorGraph createTestCase() { - GaussianFactorGraph graph; +QP createTestCase() { + QP qp; // Objective functions x1^2 - x1*x2 + x2^2 - 3*x1 + 5 // Note the Hessian encodes: // 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 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10 - graph.push_back( + qp.cost.push_back( HessianFactor(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1), 2.0 * ones(1, 1), zero(1), 10.0)); @@ -48,12 +48,9 @@ GaussianFactorGraph createTestCase() { Matrix A1 = (Matrix(4, 1) << 1, -1, 0, 1); Matrix A2 = (Matrix(4, 1) << 1, 0, -1, 0); Vector b = (Vector(4) << 2, 0, 0, 1.5); - // Special constrained noise model denoting <= inequalities with negative sigmas - noiseModel::Constrained::shared_ptr noise = - noiseModel::Constrained::MixedSigmas((Vector(4) << -1, -1, -1, -1)); - graph.push_back(JacobianFactor(X(1), A1, X(2), A2, b, noise)); + qp.inequalities.push_back(LinearInequality(X(1), A1, X(2), A2, b, 0)); - return graph; + return qp; } TEST(QPSolver, TestCase) { @@ -61,49 +58,43 @@ TEST(QPSolver, TestCase) { double x1 = 5, x2 = 7; values.insert(X(1), x1 * ones(1, 1)); values.insert(X(2), x2 * ones(1, 1)); - GaussianFactorGraph graph = createTestCase(); - DOUBLES_EQUAL(29, x1*x1 - x1*x2 + x2*x2 - 3*x1 + 5, 1e-9); - DOUBLES_EQUAL(29, graph[0]->error(values), 1e-9); + QP qp = createTestCase(); + DOUBLES_EQUAL(29, x1 * x1 - x1 * x2 + x2 * x2 - 3 * x1 + 5, 1e-9); + DOUBLES_EQUAL(29, qp.cost[0]->error(values), 1e-9); } TEST(QPSolver, constraintsAux) { - GaussianFactorGraph graph = createTestCase(); + QP qp = createTestCase(); - QPSolver solver(graph); - FastVector constraintIx = solver.constraintIndices(); - LONGS_EQUAL(1, constraintIx.size()); - LONGS_EQUAL(1, constraintIx[0]); + QPSolver solver(qp); VectorValues lambdas; - lambdas.insert(constraintIx[0], (Vector(4) << -0.5, 0.0, 0.3, 0.1)); + lambdas.insert(0, (Vector(4) << -0.5, 0.0, 0.3, 0.1)); int factorIx, lambdaIx; - boost::tie(factorIx, lambdaIx) = solver.identifyLeavingConstraint(lambdas); + boost::tie(factorIx, lambdaIx) = solver.identifyLeavingConstraint( + qp.inequalities, lambdas); LONGS_EQUAL(1, factorIx); LONGS_EQUAL(2, lambdaIx); VectorValues lambdas2; - lambdas2.insert(constraintIx[0], (Vector(4) << -0.5, 0.0, -0.3, -0.1)); + lambdas2.insert(0, (Vector(4) << -0.5, 0.0, -0.3, -0.1)); int factorIx2, lambdaIx2; boost::tie(factorIx2, lambdaIx2) = solver.identifyLeavingConstraint( - lambdas2); + qp.inequalities, lambdas2); LONGS_EQUAL(-1, factorIx2); LONGS_EQUAL(-1, lambdaIx2); - - HessianFactor expectedFreeHessian(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), - 3.0 * ones(1), 2.0 * ones(1, 1), zero(1), 1.0); - EXPECT(solver.freeHessiansOfConstrainedVars()[0]->equals(expectedFreeHessian)); } /* ************************************************************************* */ // Create a simple test graph with one equality constraint -GaussianFactorGraph createEqualityConstrainedTest() { - GaussianFactorGraph graph; +QP createEqualityConstrainedTest() { + QP qp; // Objective functions x1^2 + x2^2 // Note the Hessian encodes: // 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 - graph.push_back( + qp.cost.push_back( HessianFactor(X(1), X(2), 2.0 * ones(1, 1), zeros(1, 1), zero(1), 2.0 * ones(1, 1), zero(1), 0.0)); @@ -112,26 +103,24 @@ GaussianFactorGraph createEqualityConstrainedTest() { Matrix A1 = (Matrix(1, 1) << 1); Matrix A2 = (Matrix(1, 1) << 1); Vector b = -(Vector(1) << 1); - // Special constrained noise model denoting <= inequalities with negative sigmas - noiseModel::Constrained::shared_ptr noise = - noiseModel::Constrained::MixedSigmas((Vector(1) << 0.0)); - graph.push_back(JacobianFactor(X(1), A1, X(2), A2, b, noise)); + qp.equalities.push_back(LinearEquality(X(1), A1, X(2), A2, b, 0)); - return graph; + return qp; } TEST(QPSolver, dual) { - GaussianFactorGraph graph = createEqualityConstrainedTest(); + QP qp = createEqualityConstrainedTest(); // Initials values VectorValues initialValues; initialValues.insert(X(1), ones(1)); initialValues.insert(X(2), ones(1)); - QPSolver solver(graph); + QPSolver solver(qp); - GaussianFactorGraph dualGraph = solver.buildDualGraph(graph, initialValues); - VectorValues dual = dualGraph.optimize(); + GaussianFactorGraph::shared_ptr dualGraph = solver.buildDualGraph( + qp.inequalities, initialValues); + VectorValues dual = dualGraph->optimize(); VectorValues expectedDual; expectedDual.insert(1, (Vector(1) << 2.0)); CHECK(assert_equal(expectedDual, dual, 1e-10)); @@ -139,39 +128,36 @@ TEST(QPSolver, dual) { /* ************************************************************************* */ -TEST(QPSolver, iterate) { - GaussianFactorGraph graph = createTestCase(); - QPSolver solver(graph); - - GaussianFactorGraph workingGraph = graph.clone(); - - VectorValues currentSolution; - currentSolution.insert(X(1), zero(1)); - currentSolution.insert(X(2), zero(1)); - - std::vector expectedSolutions(3); - expectedSolutions[0].insert(X(1), (Vector(1) << 4.0 / 3.0)); - expectedSolutions[0].insert(X(2), (Vector(1) << 2.0 / 3.0)); - expectedSolutions[1].insert(X(1), (Vector(1) << 1.5)); - expectedSolutions[1].insert(X(2), (Vector(1) << 0.5)); - expectedSolutions[2].insert(X(1), (Vector(1) << 1.5)); - expectedSolutions[2].insert(X(2), (Vector(1) << 0.5)); - - bool converged = false; - int it = 0; - while (!converged) { - VectorValues lambdas; - converged = solver.iterateInPlace(workingGraph, currentSolution, lambdas); - CHECK(assert_equal(expectedSolutions[it], currentSolution, 1e-100)); - it++; - } -} - +//TEST(QPSolver, iterate) { +// QP qp = createTestCase(); +// QPSolver solver(qp); +// +// VectorValues currentSolution; +// currentSolution.insert(X(1), zero(1)); +// currentSolution.insert(X(2), zero(1)); +// +// std::vector expectedSolutions(3); +// expectedSolutions[0].insert(X(1), (Vector(1) << 4.0 / 3.0)); +// expectedSolutions[0].insert(X(2), (Vector(1) << 2.0 / 3.0)); +// expectedSolutions[1].insert(X(1), (Vector(1) << 1.5)); +// expectedSolutions[1].insert(X(2), (Vector(1) << 0.5)); +// expectedSolutions[2].insert(X(1), (Vector(1) << 1.5)); +// expectedSolutions[2].insert(X(2), (Vector(1) << 0.5)); +// +// bool converged = false; +// int it = 0; +// while (!converged) { +// VectorValues lambdas; +// converged = solver.iterateInPlace(workingGraph, currentSolution, lambdas); +// CHECK(assert_equal(expectedSolutions[it], currentSolution, 1e-100)); +// it++; +// } +//} /* ************************************************************************* */ TEST(QPSolver, optimizeForst10book_pg171Ex5) { - GaussianFactorGraph graph = createTestCase(); - QPSolver solver(graph); + QP qp = createTestCase(); + QPSolver solver(qp); VectorValues initialValues; initialValues.insert(X(1), zero(1)); initialValues.insert(X(2), zero(1)); @@ -185,14 +171,14 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) { /* ************************************************************************* */ // Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html -GaussianFactorGraph createTestMatlabQPEx() { - GaussianFactorGraph graph; +QP createTestMatlabQPEx() { + QP qp; // Objective functions 0.5*x1^2 + x2^2 - x1*x2 - 2*x1 -6*x2 // Note the Hessian encodes: // 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 - graph.push_back( + qp.cost.push_back( HessianFactor(X(1), X(2), 1.0 * ones(1, 1), -ones(1, 1), 2.0 * ones(1), 2.0 * ones(1, 1), 6 * ones(1), 1000.0)); @@ -202,17 +188,14 @@ GaussianFactorGraph createTestMatlabQPEx() { Matrix A1 = (Matrix(5, 1) << 1, -1, 2, -1, 0); Matrix A2 = (Matrix(5, 1) << 1, 2, 1, 0, -1); Vector b = (Vector(5) << 2, 2, 3, 0, 0); - // Special constrained noise model denoting <= inequalities with negative sigmas - noiseModel::Constrained::shared_ptr noise = - noiseModel::Constrained::MixedSigmas((Vector(5) << -1, -1, -1, -1, -1)); - graph.push_back(JacobianFactor(X(1), A1, X(2), A2, b, noise)); + qp.inequalities.push_back(LinearInequality(X(1), A1, X(2), A2, b, 0)); - return graph; + return qp; } TEST(QPSolver, optimizeMatlabEx) { - GaussianFactorGraph graph = createTestMatlabQPEx(); - QPSolver solver(graph); + QP qp = createTestMatlabQPEx(); + QPSolver solver(qp); VectorValues initialValues; initialValues.insert(X(1), zero(1)); initialValues.insert(X(2), zero(1)); @@ -226,33 +209,30 @@ TEST(QPSolver, optimizeMatlabEx) { /* ************************************************************************* */ // Create test graph as in Nocedal06book, Ex 16.4, pg. 475 -GaussianFactorGraph createTestNocedal06bookEx16_4() { - GaussianFactorGraph graph; +QP createTestNocedal06bookEx16_4() { + QP qp; - graph.push_back(JacobianFactor(X(1), ones(1, 1), ones(1))); - graph.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * ones(1))); + qp.cost.push_back(JacobianFactor(X(1), ones(1, 1), ones(1))); + qp.cost.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * ones(1))); // Inequality constraints - noiseModel::Constrained::shared_ptr noise = - noiseModel::Constrained::MixedSigmas((Vector(1) << -1)); - graph.push_back( - JacobianFactor(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), 2 * ones(1), - noise)); - graph.push_back( - JacobianFactor(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6 * ones(1), - noise)); - graph.push_back( - JacobianFactor(X(1), ones(1, 1), X(2), -2 * ones(1, 1), 2 * ones(1), - noise)); - graph.push_back(JacobianFactor(X(1), -ones(1, 1), zero(1), noise)); - graph.push_back(JacobianFactor(X(2), -ones(1, 1), zero(1), noise)); + qp.inequalities.push_back( + LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), 2 * ones(1), + 0)); + qp.inequalities.push_back( + LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6 * ones(1), 1)); + qp.inequalities.push_back( + LinearInequality(X(1), ones(1, 1), X(2), -2 * ones(1, 1), 2 * ones(1), + 2)); + qp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), zero(1), 3)); + qp.inequalities.push_back(LinearInequality(X(2), -ones(1, 1), zero(1), 4)); - return graph; + return qp; } TEST(QPSolver, optimizeNocedal06bookEx16_4) { - GaussianFactorGraph graph = createTestNocedal06bookEx16_4(); - QPSolver solver(graph); + QP qp = createTestNocedal06bookEx16_4(); + QPSolver solver(qp); VectorValues initialValues; initialValues.insert(X(1), (Vector(1) << 2.0)); initialValues.insert(X(2), zero(1)); @@ -288,88 +268,87 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) { 2.0000 0.5000 */ -GaussianFactorGraph modifyNocedal06bookEx16_4() { - GaussianFactorGraph graph; +QP modifyNocedal06bookEx16_4() { + QP qp; - graph.push_back(JacobianFactor(X(1), ones(1, 1), ones(1))); - graph.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * ones(1))); + qp.cost.push_back(JacobianFactor(X(1), ones(1, 1), ones(1))); + qp.cost.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * ones(1))); // Inequality constraints noiseModel::Constrained::shared_ptr noise = noiseModel::Constrained::MixedSigmas((Vector(1) << -1)); - graph.push_back( - JacobianFactor(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), -1 * ones(1), - noise)); - graph.push_back( - JacobianFactor(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6 * ones(1), - noise)); - graph.push_back( - JacobianFactor(X(1), ones(1, 1), X(2), -2 * ones(1, 1), 2 * ones(1), - noise)); - graph.push_back(JacobianFactor(X(1), -ones(1, 1), zero(1), noise)); - graph.push_back(JacobianFactor(X(2), -ones(1, 1), zero(1), noise)); + qp.inequalities.push_back( + LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), -1 * ones(1), + 0)); + qp.inequalities.push_back( + LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6 * ones(1), 1)); + qp.inequalities.push_back( + LinearInequality(X(1), ones(1, 1), X(2), -2 * ones(1, 1), 2 * ones(1), + 2)); + qp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), zero(1), 3)); + qp.inequalities.push_back(LinearInequality(X(2), -ones(1, 1), zero(1), 4)); - return graph; + return qp; } TEST(QPSolver, optimizeNocedal06bookEx16_4_findInitialPoint) { - GaussianFactorGraph graph = modifyNocedal06bookEx16_4(); - QPSolver solver(graph); + QP qp = modifyNocedal06bookEx16_4(); + QPSolver solver(qp); VectorValues initialsLP; - Key firstSlackKey; - boost::tie(initialsLP, firstSlackKey) = solver.initialValuesLP(); + Key firstSlackKey, lastSlackKey; + boost::tie(initialsLP, firstSlackKey, lastSlackKey) = solver.initialValuesLP(); EXPECT(assert_equal(zero(1), initialsLP.at(X(1)))); EXPECT(assert_equal(zero(1), initialsLP.at(X(2)))); - LONGS_EQUAL(X(2)+1, firstSlackKey); + LONGS_EQUAL(X(2) + 1, firstSlackKey); EXPECT(assert_equal(zero(1), initialsLP.at(firstSlackKey))); - EXPECT(assert_equal(ones(1)*6.0, initialsLP.at(firstSlackKey+1))); - EXPECT(assert_equal(ones(1)*2.0, initialsLP.at(firstSlackKey+2))); - EXPECT(assert_equal(zero(1), initialsLP.at(firstSlackKey+3))); - EXPECT(assert_equal(zero(1), initialsLP.at(firstSlackKey+4))); + EXPECT(assert_equal(ones(1) * 6.0, initialsLP.at(firstSlackKey + 1))); + EXPECT(assert_equal(ones(1) * 2.0, initialsLP.at(firstSlackKey + 2))); + EXPECT(assert_equal(zero(1), initialsLP.at(firstSlackKey + 3))); + EXPECT(assert_equal(zero(1), initialsLP.at(firstSlackKey + 4))); VectorValues objCoeffs = solver.objectiveCoeffsLP(firstSlackKey); for (size_t i = 0; i < 5; ++i) - EXPECT(assert_equal(ones(1), objCoeffs.at(firstSlackKey+i))); + EXPECT(assert_equal(ones(1), objCoeffs.at(firstSlackKey + i))); - GaussianFactorGraph::shared_ptr constraints; + LinearEqualityFactorGraph::shared_ptr equalities; + LinearInequalityFactorGraph::shared_ptr inequalities; VectorValues lowerBounds; - boost::tie(constraints, lowerBounds) = solver.constraintsLP(firstSlackKey); + boost::tie(equalities, inequalities, lowerBounds) = solver.constraintsLP( + firstSlackKey); for (size_t i = 0; i < 5; ++i) - EXPECT(assert_equal(zero(1), lowerBounds.at(firstSlackKey+i))); + EXPECT(assert_equal(zero(1), lowerBounds.at(firstSlackKey + i))); - GaussianFactorGraph expectedConstraints; - noiseModel::Constrained::shared_ptr noise = - noiseModel::Constrained::MixedSigmas((Vector(1) << -1)); - expectedConstraints.push_back( - JacobianFactor(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), X(3), -ones(1, 1), - -1 * ones(1), noise)); - expectedConstraints.push_back( - JacobianFactor(X(1), ones(1, 1), X(2), 2 * ones(1, 1), X(4), -ones(1, 1), - 6 * ones(1), noise)); - expectedConstraints.push_back( - JacobianFactor(X(1), ones(1, 1), X(2), -2 * ones(1, 1), X(5), -ones(1, 1), - 2 * ones(1), noise)); - expectedConstraints.push_back( - JacobianFactor(X(1), -ones(1, 1), X(6), -ones(1, 1), zero(1), noise)); - expectedConstraints.push_back( - JacobianFactor(X(2), -ones(1, 1), X(7), -ones(1, 1), zero(1), noise)); - EXPECT(assert_equal(expectedConstraints, *constraints)); + LinearInequalityFactorGraph expectedInequalities; + expectedInequalities.push_back( + LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), X(3), + -ones(1, 1), -1 * ones(1), 0)); + expectedInequalities.push_back( + LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), X(4), + -ones(1, 1), 6 * ones(1), 1)); + expectedInequalities.push_back( + LinearInequality(X(1), ones(1, 1), X(2), -2 * ones(1, 1), X(5), + -ones(1, 1), 2 * ones(1), 2)); + expectedInequalities.push_back( + LinearInequality(X(1), -ones(1, 1), X(6), -ones(1, 1), zero(1), 3)); + expectedInequalities.push_back( + LinearInequality(X(2), -ones(1, 1), X(7), -ones(1, 1), zero(1), 4)); + EXPECT(assert_equal(expectedInequalities, *inequalities)); bool isFeasible; 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)))); + 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(); - EXPECT(assert_equal(2.0*ones(1), solution.at(X(1)))); - EXPECT(assert_equal(0.5*ones(1), solution.at(X(2)))); + EXPECT(assert_equal(2.0 * ones(1), solution.at(X(1)))); + EXPECT(assert_equal(0.5 * ones(1), solution.at(X(2)))); } TEST(QPSolver, optimizeNocedal06bookEx16_4_2) { - GaussianFactorGraph graph = createTestNocedal06bookEx16_4(); - QPSolver solver(graph); + QP qp = createTestNocedal06bookEx16_4(); + QPSolver solver(qp); VectorValues initialValues; initialValues.insert(X(1), (Vector(1) << 0.0)); initialValues.insert(X(2), (Vector(1) << 100.0)); @@ -391,17 +370,16 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4_2) { /* ************************************************************************* */ TEST(QPSolver, failedSubproblem) { - GaussianFactorGraph graph; - graph.push_back(JacobianFactor(X(1), eye(2), zero(2))); - graph.push_back(HessianFactor(X(1), zeros(2, 2), zero(2), 100.0)); - graph.push_back( - JacobianFactor(X(1), (Matrix(1, 2) << -1.0, 0.0), -ones(1), - noiseModel::Constrained::MixedSigmas(-ones(1)))); + QP qp; + qp.cost.push_back(JacobianFactor(X(1), eye(2), zero(2))); + qp.cost.push_back(HessianFactor(X(1), zeros(2, 2), zero(2), 100.0)); + qp.inequalities.push_back( + LinearInequality(X(1), (Matrix(1, 2) << -1.0, 0.0), -ones(1), 0)); VectorValues expected; expected.insert(X(1), (Vector(2) << 1.0, 0.0)); - QPSolver solver(graph); + QPSolver solver(qp); VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize(); // graph.print("Graph: "); From 57aae7f7b1603dfa581b44854162dc9cc7512a53 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Tue, 9 Dec 2014 10:25:59 -0500 Subject: [PATCH 09/13] revert .cproject to its previous version in commit a9e3545a --- .cproject | 2138 ++++++++++++++++++++++++++++------------------------- 1 file changed, 1119 insertions(+), 1019 deletions(-) diff --git a/.cproject b/.cproject index 7ae530863..30c3d3327 100644 --- a/.cproject +++ b/.cproject @@ -1,19 +1,17 @@ - - - + - - + + @@ -62,13 +60,13 @@ - - + + @@ -118,13 +116,13 @@ - - + + @@ -584,6 +582,7 @@ make + tests/testBayesTree.run true false @@ -591,6 +590,7 @@ make + testBinaryBayesNet.run true false @@ -638,6 +638,7 @@ make + testSymbolicBayesNet.run true false @@ -645,6 +646,7 @@ make + tests/testSymbolicFactor.run true false @@ -652,6 +654,7 @@ make + testSymbolicFactorGraph.run true false @@ -667,11 +670,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -728,6 +740,46 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + make -j2 @@ -736,7 +788,31 @@ true true - + + make + -j5 + testGaussianFactorGraphUnordered.run + true + true + true + + + make + -j5 + testGaussianBayesNetUnordered.run + true + true + true + + + make + -j2 + clean + true + true + true + + make -j2 all @@ -744,18 +820,82 @@ true true - + make -j2 - check + clean true true true - + make -j2 - clean + clean all + true + true + true + + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j6 -j8 + testAntiFactor.run + true + true + true + + + make + -j6 -j8 + testBetweenFactor.run + true + true + true + + + make + -j5 + testDataset.run + true + true + true + + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j5 + testRotateFactor.run true true true @@ -800,6 +940,30 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + make -j5 @@ -842,23 +1006,40 @@ make + testErrors.run true true true - + make - -j5 - testGaussianFactorGraphUnordered.run + -j2 + check true true true - + make - -j5 - testGaussianBayesNetUnordered.run + -j2 + timeCalibratedCamera.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + clean true true true @@ -927,6 +1108,379 @@ true true + + make + -j5 + testDiscreteFactor.run + true + true + true + + + make + -j1 + testDiscreteBayesTree.run + true + false + true + + + make + -j5 + testDiscreteFactorGraph.run + true + true + true + + + make + -j5 + testDiscreteConditional.run + true + true + true + + + make + -j5 + testDiscreteMarginals.run + true + true + true + + + make + -j5 + testInference.run + true + true + true + + + make + -j1 + testSymbolicSequentialSolver.run + true + false + true + + + make + -j5 + testEliminationTree.run + true + true + true + + + make + -j1 + testSymbolicBayesTree.run + true + false + true + + + make + -j2 + vSFMexample.run + true + true + true + + + make + -j5 + testInvDepthCamera3.run + true + true + true + + + make + -j5 + testTriangulation.run + true + true + true + + + make + -j2 + testVSLAMGraph + true + true + true + + + make + -j5 + check.tests + true + true + true + + + make + -j2 + timeGaussianFactorGraph.run + true + true + true + + + make + -j5 + testMarginals.run + true + true + true + + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + + + make + -j5 + testGaussianISAM2.run + true + true + true + + + make + -j5 + testSymbolicFactorGraphB.run + true + true + true + + + make + -j2 + timeSequentialOnDataset.run + true + true + true + + + make + -j5 + testGradientDescentOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + testNonlinearOptimizer.run + true + true + true + + + make + -j2 + testGaussianBayesNet.run + true + true + true + + + make + -j2 + testNonlinearISAM.run + true + true + true + + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testExtendedKalmanFilter.run + true + true + true + + + make + testSimulated2DOriented.run + true + false + true + + + make + -j5 + timing.tests + true + true + true + + + make + -j5 + testNonlinearFactor.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + testGaussianJunctionTreeB.run + true + true + true + + + make + testSimulated2D.run + true + false + true + + + make + testSimulated3D.run + true + false + true + + + make + + testSymbolicBayesNetB.run + true + false + true + + + make + -j5 + testEliminationTree.run + true + true + true + + + make + -j5 + testInference.run + true + true + true + + + make + -j5 + testKey.run + true + true + true + + + make + -j1 + testSymbolicBayesTree.run + true + false + true + + + make + -j1 + testSymbolicSequentialSolver.run + true + false + true + + + make + -j5 + testGaussianFactorGraphB.run + true + true + true + + + make + -j5 + testSummarization.run + true + true + true + + + make + -j5 + timeIncremental.run + true + true + true + make -j5 @@ -1033,6 +1587,7 @@ make + testErrors.run true false @@ -1118,6 +1673,38 @@ false true + + cpack + + -G DEB + true + false + true + + + cpack + + -G RPM + true + false + true + + + cpack + + -G TGZ + true + false + true + + + cpack + + --config CPackSourceConfig.cmake + true + true + true + make -j2 @@ -1158,6 +1745,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -1262,6 +1857,54 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -1270,66 +1913,130 @@ true true - + make -j5 - testBTree.run + testStereoCamera.run true true true - + make -j5 - testDSF.run + testRot3M.run true true true - + make -j5 - testDSFMap.run + testPoint3.run true true true - + make -j5 - testDSFVector.run + testCalibratedCamera.run true true true - + make -j5 - testFixedVector.run + timeStereoCamera.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + timeCalibratedCamera.run + true + true + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + timePinholeCamera.run + true + true + true + + + make + -j5 + testCal3DS2.run + true + true + true + + + make + -j5 + testCal3Bundler.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testEssentialMatrix.run true true true make - -j5 - testGaussianISAM2.run - true - true - true - - - make - - testSimulated2DOriented.run - true - false - true - - - make - -j5 - timing.tests + -j2 + all true true true @@ -1406,6 +2113,21 @@ true true + + make + testSimulated2DOriented.run + true + false + true + + + make + -j2 + testVSLAMConfig.run + true + true + true + make -j2 @@ -1432,7 +2154,6 @@ make - testSimulated2D.run true false @@ -1440,7 +2161,6 @@ make - testSimulated3D.run true false @@ -1454,46 +2174,22 @@ true true - + make -j5 - testEliminationTree.run + testVector.run true true true - + make -j5 - testInference.run + testMatrix.run true true true - - make - -j5 - testKey.run - true - true - true - - - make - -j1 - testSymbolicBayesTree.run - true - false - true - - - make - -j1 - testSymbolicSequentialSolver.run - true - false - true - make -j5 @@ -1582,10 +2278,193 @@ true true - + + make + -j2 + SimpleRotation.run + true + true + true + + make -j5 - testGaussianFactorGraphB.run + CameraResectioning.run + true + true + true + + + make + -j5 + PlanarSLAMExample.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + easyPoint2KalmanFilter.run + true + true + true + + + make + -j2 + elaboratePoint2KalmanFilter.run + true + true + true + + + make + -j5 + Pose2SLAMExample.run + true + true + true + + + make + -j2 + Pose2SLAMwSPCG_easy.run + true + true + true + + + make + -j5 + UGM_small.run + true + true + true + + + make + -j5 + LocalizationExample.run + true + true + true + + + make + -j5 + OdometryExample.run + true + true + true + + + make + -j5 + RangeISAMExample_plaza2.run + true + true + true + + + make + + testGraph.run + true + false + true + + + make + + testJunctionTree.run + true + true + true + + + make + + testSymbolicBayesNetB.run + true + true + true + + + make + -j5 + VisualISAMExample.run + true + true + true + + + make + -j5 + VisualISAM2Example.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graphviz.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graph.run + true + true + true + + + make + -j5 + SFMExample_bal.run + true + true + true + + + make + -j4 + testImuFactor.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + tests/testGaussianISAM2 + true + false + true + + + make + -j2 + check true true true @@ -1598,34 +2477,6 @@ true true - - cpack - -G DEB - true - false - true - - - cpack - -G RPM - true - false - true - - - cpack - -G TGZ - true - false - true - - - cpack - --config CPackSourceConfig.cmake - true - true - true - make -j3 @@ -1729,6 +2580,38 @@ true true + + make + -j5 + testGPSFactor.run + true + true + true + + + make + -j5 + testGaussMarkov1stOrderFactor.run + true + true + true + + + make + -j5 + testImplicitSchurFactor.run + true + true + true + + + make + -j2 VERBOSE=1 + check.geometry + true + false + true + make -j5 @@ -1817,6 +2700,38 @@ false true + + cpack + + -G DEB + true + false + true + + + cpack + + -G RPM + true + false + true + + + cpack + + -G TGZ + true + false + true + + + cpack + + --config CPackSourceConfig.cmake + true + false + true + make -j5 @@ -1971,921 +2886,11 @@ make - -j5 tests/testGaussianISAM2 true true true - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. - wrap - true - false - true - - - make - -j5 - testQPSolver.run - true - true - true - - - make - -j5 - testRot2.run - true - true - true - - - cmake-gui - .. - true - false - true - - - make - -j5 - testClp.run - true - true - true - - - make - -j5 - testlpsolve.run - true - true - true - - - make - -j5 - testLPSolver.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - all - true - false - true - - - make - -j5 - check - true - false - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - clean all - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j6 -j8 - testAntiFactor.run - true - true - true - - - make - -j6 -j8 - testBetweenFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - timeCalibratedCamera.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testDiscreteFactor.run - true - true - true - - - make - -j1 - testDiscreteBayesTree.run - true - false - true - - - make - -j5 - testDiscreteFactorGraph.run - true - true - true - - - make - -j5 - testDiscreteConditional.run - true - true - true - - - make - -j5 - testDiscreteMarginals.run - true - true - true - - - make - -j2 - vSFMexample.run - true - true - true - - - make - -j5 - testInference.run - true - true - true - - - make - -j1 - testSymbolicSequentialSolver.run - true - false - true - - - make - -j5 - testEliminationTree.run - true - true - true - - - make - -j1 - testSymbolicBayesTree.run - true - false - true - - - make - -j2 - testVSLAMGraph - true - true - true - - - make - -j5 - testInvDepthCamera3.run - true - true - true - - - make - -j5 - testTriangulation.run - true - true - true - - - make - -j5 - check.tests - true - true - true - - - make - -j2 - timeGaussianFactorGraph.run - true - true - true - - - make - -j5 - testMarginals.run - true - true - true - - - make - -j5 - testSymbolicFactorGraphB.run - true - true - true - - - make - -j2 - timeSequentialOnDataset.run - true - true - true - - - make - -j5 - testGradientDescentOptimizer.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - testNonlinearOptimizer.run - true - true - true - - - make - -j2 - testGaussianBayesNet.run - true - true - true - - - make - -j2 - testNonlinearISAM.run - true - true - true - - - make - -j2 - testNonlinearEquality.run - true - true - true - - - make - -j2 - testExtendedKalmanFilter.run - true - true - true - - - make - -j5 - testNonlinearFactor.run - true - true - true - - - make - -j5 - clean - true - true - true - - - make - -j5 - testGaussianJunctionTreeB.run - true - true - true - - - make - - testSimulated2D.run - true - false - true - - - make - - testSimulated3D.run - true - false - true - - - make - testSymbolicBayesNetB.run - true - false - true - - - make - -j5 - testSummarization.run - true - true - true - - - make - -j5 - timeIncremental.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testGPSFactor.run - true - true - true - - - make - -j5 - testGaussMarkov1stOrderFactor.run - true - true - true - - - make - -j5 - testImplicitSchurFactor.run - true - true - true - - - make - -j5 - testStereoCamera.run - true - true - true - - - make - -j5 - testRot3M.run - true - true - true - - - make - -j5 - testPoint3.run - true - true - true - - - make - -j5 - testCalibratedCamera.run - true - true - true - - - make - -j5 - timeStereoCamera.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - timeCalibratedCamera.run - true - true - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - testCal3DS2.run - true - true - true - - - make - -j5 - testCal3Bundler.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testEssentialMatrix.run - true - true - true - - - make - -j5 - testQPSolver.run - true - true - true - - - make - -j5 - testVector.run - true - true - true - - - make - -j5 - testMatrix.run - true - true - true - - - make - -j2 - SimpleRotation.run - true - true - true - - - make - -j5 - CameraResectioning.run - true - true - true - - - make - -j5 - PlanarSLAMExample.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - easyPoint2KalmanFilter.run - true - true - true - - - make - -j2 - elaboratePoint2KalmanFilter.run - true - true - true - - - make - -j5 - Pose2SLAMExample.run - true - true - true - - - make - -j2 - Pose2SLAMwSPCG_easy.run - true - true - true - - - make - -j5 - UGM_small.run - true - true - true - - - make - -j5 - LocalizationExample.run - true - true - true - - - make - -j5 - OdometryExample.run - true - true - true - - - make - -j5 - RangeISAMExample_plaza2.run - true - true - true - - - make - testSymbolicBayesNetB.run - true - true - true - - - make - -j5 - VisualISAMExample.run - true - true - true - - - make - -j5 - VisualISAM2Example.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graphviz.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graph.run - true - true - true - - - make - -j5 - SFMExample_bal.run - true - true - true - - - make - -j4 - testImuFactor.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - - tests/testGaussianISAM2 - true - false - true - make -j2 @@ -2982,10 +2987,105 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + + + make + install + testSpirit.run + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. + wrap + true + false + true + + + make + -j5 + testQPSolver.run + true + true + true + + + make + -j5 + testRot2.run + true + true + true + + + cmake-gui + .. + true + false + true + + + make + -j5 + testClp.run + true + true + true + + + make + -j5 + testlpsolve.run + true + true + true + + + make + -j5 + testLPSolver.run + true + true + true + make -j5 - install + testSpirit.run true true true From 9b418c98ca0443a7edd0cf65fe4f0b772e5365b5 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Tue, 9 Dec 2014 16:26:19 -0500 Subject: [PATCH 10/13] fix LPSolver unittest --- gtsam_unstable/linear/tests/testLPSolver.cpp | 25 ++++++++++---------- 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp index e8606293e..d8f5142e2 100644 --- a/gtsam_unstable/linear/tests/testLPSolver.cpp +++ b/gtsam_unstable/linear/tests/testLPSolver.cpp @@ -10,6 +10,7 @@ #include #include #include +#include using namespace std; using namespace gtsam; @@ -20,19 +21,19 @@ TEST(LPSolver, oneD) { objCoeffs.insert(Y(1), repeat(1, -5.0)); objCoeffs.insert(X(2), repeat(1, -4.0)); objCoeffs.insert(X(3), repeat(1, -6.0)); - JacobianFactor constraint( + LinearInequality inequality( Y(1), (Matrix(3,1)<< 1,3,3), X(2), (Matrix(3,1)<< -1,2,2), - X(3), (Matrix(3,1)<< 1,4,0), (Vector(3)<<20,42,30), - noiseModel::Constrained::MixedSigmas((Vector(3)<<-1,-1,-1))); + X(3), (Matrix(3,1)<< 1,4,0), (Vector(3)<<20,42,30), 0); VectorValues lowerBounds; lowerBounds.insert(Y(1), zero(1)); lowerBounds.insert(X(2), zero(1)); lowerBounds.insert(X(3), zero(1)); - GaussianFactorGraph::shared_ptr constraints(new GaussianFactorGraph); - constraints->push_back(constraint); + LinearInequalityFactorGraph::shared_ptr inequalities(new LinearInequalityFactorGraph); + inequalities->push_back(inequality); + LinearEqualityFactorGraph::shared_ptr equalities(new LinearEqualityFactorGraph); - LPSolver solver(objCoeffs, constraints, lowerBounds); + LPSolver solver(objCoeffs, equalities, inequalities, lowerBounds); vector columnNo = solver.buildColumnNo(objCoeffs | boost::adaptors::map_keys); LONGS_EQUAL(3, columnNo.size()); LONGS_EQUAL(1, columnNo[0]); @@ -64,16 +65,16 @@ TEST(LPSolver, oneD) { TEST(LPSolver, threeD) { VectorValues objCoeffs; objCoeffs.insert(X(1), (Vector(3)<<-5.0, -4.0, -6.0)); - JacobianFactor constraint( - X(1), (Matrix(3,3)<< 1,-1,1,3,2,4,3,2,0), (Vector(3)<<20,42,30), - noiseModel::Constrained::MixedSigmas((Vector(3)<<-1,-1,-1))); + LinearInequality inequality( + X(1), (Matrix(3,3)<< 1,-1,1,3,2,4,3,2,0), (Vector(3)<<20,42,30), 0); VectorValues lowerBounds; lowerBounds.insert(X(1), zeros(3,1)); - GaussianFactorGraph::shared_ptr constraints(new GaussianFactorGraph); - constraints->push_back(constraint); + LinearInequalityFactorGraph::shared_ptr inequalities(new LinearInequalityFactorGraph); + inequalities->push_back(inequality); + LinearEqualityFactorGraph::shared_ptr equalities(new LinearEqualityFactorGraph); - LPSolver solver(objCoeffs, constraints, lowerBounds); + LPSolver solver(objCoeffs, equalities, inequalities, lowerBounds); vector columnNo = solver.buildColumnNo(objCoeffs | boost::adaptors::map_keys); LONGS_EQUAL(3, columnNo.size()); LONGS_EQUAL(1, columnNo[0]); From 85397223efca171afe6dfe7d227f6b2bc8e7ad67 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Tue, 9 Dec 2014 16:27:11 -0500 Subject: [PATCH 11/13] fix QPSolver unit tests --- .../linear/LinearInequalityFactorGraph.h | 1 - gtsam_unstable/linear/QPSolver.cpp | 58 ++++++++-- gtsam_unstable/linear/QPSolver.h | 29 ++++- gtsam_unstable/linear/tests/testQPSolver.cpp | 100 ++++++++++++------ 4 files changed, 144 insertions(+), 44 deletions(-) diff --git a/gtsam_unstable/linear/LinearInequalityFactorGraph.h b/gtsam_unstable/linear/LinearInequalityFactorGraph.h index bfc64df2a..b72f2cc3c 100644 --- a/gtsam_unstable/linear/LinearInequalityFactorGraph.h +++ b/gtsam_unstable/linear/LinearInequalityFactorGraph.h @@ -18,7 +18,6 @@ #pragma once -#include #include #include diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index 7872ce27e..57c20d897 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -14,9 +14,6 @@ using namespace std; -#define ACTIVE 0.0 -#define INACTIVE std::numeric_limits::infinity() - namespace gtsam { //****************************************************************************** @@ -52,11 +49,29 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key, // Collect the gradients of unconstrained cost factors to the b vector Vector b = zero(delta.at(key).size()); - BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) { - GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); - b += factor->gradient(key, delta); + if (costVariableIndex_.find(key) != costVariableIndex_.end()) { + BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) { + GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); + b += factor->gradient(key, delta); + } } - return boost::make_shared(Aterms, b); + + return boost::make_shared(Aterms, b, noiseModel::Constrained::All(b.rows())); +} + +//****************************************************************************** +GaussianFactor::shared_ptr QPSolver::createDualPrior( + const LinearInequality::shared_ptr& factor) const { + Vector sigmas = factor->get_model()->sigmas(); + size_t n = sigmas.rows(); + Matrix A = eye(n); + for (size_t i = 0; i(factor->dualKey(), A, b); } //****************************************************************************** @@ -67,6 +82,11 @@ GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph( // Each constrained key becomes a factor in the dual graph dualGraph->push_back(createDualFactor(key, workingSet, delta)); } + + // Add prior for inactive dual variables + BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) { + dualGraph->push_back(createDualPrior(factor)); + } return dualGraph; } @@ -224,13 +244,33 @@ QPState QPSolver::iterate(const QPState& state) const { } } +//****************************************************************************** +LinearInequalityFactorGraph QPSolver::identifyActiveConstraints( + const LinearInequalityFactorGraph& inequalities, + const VectorValues& initialValues) const { + LinearInequalityFactorGraph workingSet; + BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities){ + LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); + Vector e = workingFactor->error_vector(initialValues); + Vector sigmas = zero(e.rows()); + for (int i = 0; i < e.rows(); ++i){ + if (fabs(e[i])>1e-7){ + sigmas[i] = INACTIVE; + } + } + workingFactor->setModel(true, sigmas); + workingSet.push_back(workingFactor); + } + return workingSet; +} + //****************************************************************************** pair QPSolver::optimize( const VectorValues& initialValues) const { // TODO: initialize workingSet from the feasible initialValues - LinearInequalityFactorGraph workingSet(qp_.inequalities); - + LinearInequalityFactorGraph workingSet = + identifyActiveConstraints(qp_.inequalities, initialValues); QPState state(initialValues, VectorValues(), workingSet, false); /// main loop of the solver diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index b3c66a21f..85a387140 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -13,6 +13,9 @@ #include #include +#define ACTIVE 0.0 +#define INACTIVE std::numeric_limits::infinity() + namespace gtsam { /// This struct holds the state of QPSolver at each iteration @@ -67,10 +70,12 @@ public: const FactorGraph& graph, const VariableIndex& variableIndex) const { std::vector > Aterms; - BOOST_FOREACH(size_t factorIx, variableIndex[key]){ - typename FACTOR::shared_ptr factor = graph.at(factorIx); - Matrix Ai = factor->getA(factor->find(key)).transpose(); - Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); + if (variableIndex.find(key) != variableIndex.end()) { + BOOST_FOREACH(size_t factorIx, variableIndex[key]){ + typename FACTOR::shared_ptr factor = graph.at(factorIx); + Matrix Ai = factor->getA(factor->find(key)).transpose(); + Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); + } } return Aterms; } @@ -80,6 +85,15 @@ public: const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const; + /** + * Create dummy prior for inactive dual variables + * TODO: This might be inefficient but I don't know how to avoid + * the Indeterminant exception due to no information for the duals + * on inactive components of the constraints. + */ + GaussianFactor::shared_ptr createDualPrior( + const LinearInequality::shared_ptr& factor) const; + /** * Build the dual graph to solve for the Lagrange multipliers. * @@ -173,6 +187,13 @@ public: /** Iterate 1 step, return a new state with a new workingSet and values */ QPState iterate(const QPState& state) const; + /** + * Identify active constraints based on initial values. + */ + LinearInequalityFactorGraph identifyActiveConstraints( + const LinearInequalityFactorGraph& inequalities, + const VectorValues& initialValues) const; + /** Optimize with a provided initial values * For this version, it is the responsibility of the caller to provide * a feasible initial value, otherwise the solution will be wrong. diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index edf64655d..6ac329688 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -26,9 +26,6 @@ using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; -#define TEST_DISABLED(testGroup, testName)\ - void testGroup##testName##Test(TestResult& result_, const std::string& name_) - /* ************************************************************************* */ // Create test graph according to Forst10book_pg171Ex5 QP createTestCase() { @@ -73,7 +70,7 @@ TEST(QPSolver, constraintsAux) { int factorIx, lambdaIx; boost::tie(factorIx, lambdaIx) = solver.identifyLeavingConstraint( qp.inequalities, lambdas); - LONGS_EQUAL(1, factorIx); + LONGS_EQUAL(0, factorIx); LONGS_EQUAL(2, lambdaIx); VectorValues lambdas2; @@ -122,37 +119,80 @@ TEST(QPSolver, dual) { qp.inequalities, initialValues); VectorValues dual = dualGraph->optimize(); VectorValues expectedDual; - expectedDual.insert(1, (Vector(1) << 2.0)); + expectedDual.insert(0, (Vector(1) << 2.0)); CHECK(assert_equal(expectedDual, dual, 1e-10)); } /* ************************************************************************* */ +TEST(QPSolver, indentifyActiveConstraints) { + QP qp = createTestCase(); + QPSolver solver(qp); + + VectorValues currentSolution; + currentSolution.insert(X(1), zero(1)); + currentSolution.insert(X(2), zero(1)); + + LinearInequalityFactorGraph workingSet = + solver.identifyActiveConstraints(qp.inequalities, currentSolution); + + Vector actualSigmas = workingSet.at(0)->get_model()->sigmas(); + Vector expectedSigmas = (Vector(4) << INACTIVE, ACTIVE, ACTIVE, INACTIVE); + CHECK(assert_equal(expectedSigmas, actualSigmas, 1e-100)); + + VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet); + VectorValues expectedSolution; + expectedSolution.insert(X(1), (Vector(1) << 0.0)); + expectedSolution.insert(X(2), (Vector(1) << 0.0)); + CHECK(assert_equal(expectedSolution, solution, 1e-100)); + +} + +/* ************************************************************************* */ +TEST(QPSolver, iterate) { + QP qp = createTestCase(); + QPSolver solver(qp); + + VectorValues currentSolution; + currentSolution.insert(X(1), zero(1)); + currentSolution.insert(X(2), zero(1)); + + std::vector expectedSolutions(4), expectedDuals(4); + expectedSolutions[0].insert(X(1), (Vector(1) << 0.0)); + expectedSolutions[0].insert(X(2), (Vector(1) << 0.0)); + expectedDuals[0].insert(0, (Vector(4) << 0, 3, 0, 0)); + + expectedSolutions[1].insert(X(1), (Vector(1) << 1.5)); + expectedSolutions[1].insert(X(2), (Vector(1) << 0.0)); + expectedDuals[1].insert(0, (Vector(4) << 0, 0, 1.5, 0)); + + expectedSolutions[2].insert(X(1), (Vector(1) << 1.5)); + expectedSolutions[2].insert(X(2), (Vector(1) << 0.75)); + expectedDuals[2].insert(0, (Vector(4) << 0, 0, 1.5, 0)); + + expectedSolutions[3].insert(X(1), (Vector(1) << 1.5)); + expectedSolutions[3].insert(X(2), (Vector(1) << 0.5)); + expectedDuals[3].insert(0, (Vector(4) << -0.5, 0, 0, 0)); + + LinearInequalityFactorGraph workingSet = + solver.identifyActiveConstraints(qp.inequalities, currentSolution); + + QPState state(currentSolution, VectorValues(), workingSet, false); + + int it = 0; + while (!state.converged) { + state = solver.iterate(state); + // These checks will fail because the expected solutions obtained from + // Forst10book do not follow exactly what we implemented from Nocedal06book. + // Specifically, we do not re-identify active constraints and + // do not recompute dual variables after every step!!! +// CHECK(assert_equal(expectedSolutions[it], state.values, 1e-10)); +// CHECK(assert_equal(expectedDuals[it], state.duals, 1e-10)); + it++; + } + + CHECK(assert_equal(expectedSolutions[3], state.values, 1e-10)); +} -//TEST(QPSolver, iterate) { -// QP qp = createTestCase(); -// QPSolver solver(qp); -// -// VectorValues currentSolution; -// currentSolution.insert(X(1), zero(1)); -// currentSolution.insert(X(2), zero(1)); -// -// std::vector expectedSolutions(3); -// expectedSolutions[0].insert(X(1), (Vector(1) << 4.0 / 3.0)); -// expectedSolutions[0].insert(X(2), (Vector(1) << 2.0 / 3.0)); -// expectedSolutions[1].insert(X(1), (Vector(1) << 1.5)); -// expectedSolutions[1].insert(X(2), (Vector(1) << 0.5)); -// expectedSolutions[2].insert(X(1), (Vector(1) << 1.5)); -// expectedSolutions[2].insert(X(2), (Vector(1) << 0.5)); -// -// bool converged = false; -// int it = 0; -// while (!converged) { -// VectorValues lambdas; -// converged = solver.iterateInPlace(workingGraph, currentSolution, lambdas); -// CHECK(assert_equal(expectedSolutions[it], currentSolution, 1e-100)); -// it++; -// } -//} /* ************************************************************************* */ TEST(QPSolver, optimizeForst10book_pg171Ex5) { From 565eb999480036bb53138635286f97e1cc0d602d Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Tue, 9 Dec 2014 22:28:39 -0500 Subject: [PATCH 12/13] remove a TODO as it's done. --- gtsam_unstable/linear/QPSolver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index 57c20d897..4081c4d16 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -268,7 +268,7 @@ LinearInequalityFactorGraph QPSolver::identifyActiveConstraints( pair QPSolver::optimize( const VectorValues& initialValues) const { - // TODO: initialize workingSet from the feasible initialValues + // Initialize workingSet from the feasible initialValues LinearInequalityFactorGraph workingSet = identifyActiveConstraints(qp_.inequalities, initialValues); QPState state(initialValues, VectorValues(), workingSet, false); From ba903536c8f74185e7a037363be422ff16b5dcc0 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Tue, 9 Dec 2014 22:29:21 -0500 Subject: [PATCH 13/13] fix ACTIVE/INACTIVE constants as Frank suggested --- gtsam_unstable/linear/QPSolver.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index 85a387140..8d51322ae 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -13,11 +13,11 @@ #include #include -#define ACTIVE 0.0 -#define INACTIVE std::numeric_limits::infinity() - namespace gtsam { +static const double ACTIVE = 0.0; +static const double INACTIVE = std::numeric_limits::infinity(); + /// This struct holds the state of QPSolver at each iteration struct QPState { VectorValues values;