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)); } /* ************************************************************************* */