Simplified freeHessians_ using inner class
							parent
							
								
									b5e8be56f3
								
							
						
					
					
						commit
						9ca2ba9b66
					
				|  | @ -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<JacobianFactor>(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<Key>& constrainedVars) const { | ||||
|   VariableIndex variableIndex(graph); | ||||
|   GaussianFactorGraph::shared_ptr hfg(new GaussianFactorGraph()); | ||||
| //******************************************************************************
 | ||||
| void QPSolver::findUnconstrainedHessiansOfConstrainedVars( | ||||
|     const set<Key>& constrainedVars) { | ||||
|   VariableIndex variableIndex(graph_); | ||||
| 
 | ||||
|   // Collect all factors involving constrained vars
 | ||||
|   FastSet<size_t> 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<JacobianFactor>(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<HessianFactor>(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<int, int> QPSolver::findWorstViolatedActiveIneq( | ||||
|     const VectorValues& lambdas) const { | ||||
|   int worstFactorIx = -1, worstSigmaIx = -1; | ||||
|  | @ -253,9 +263,9 @@ pair<int, int> 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<int, int> 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<double, int, int> 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<double, int, int> QPSolver::computeStepSize( | |||
|   return false; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| //******************************************************************************
 | ||||
| pair<VectorValues, VectorValues> QPSolver::optimize( | ||||
|     const VectorValues& initials) const { | ||||
|   GaussianFactorGraph workingGraph = graph_.clone(); | ||||
|  | @ -413,7 +423,7 @@ pair<VectorValues, VectorValues> QPSolver::optimize( | |||
|   return make_pair(currentSolution, lambdas); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| //******************************************************************************
 | ||||
| pair<VectorValues, Key> QPSolver::initialValuesLP() const { | ||||
|   size_t firstSlackKey = 0; | ||||
|   BOOST_FOREACH(Key key, fullFactorIndices_ | boost::adaptors::map_keys) { | ||||
|  | @ -455,7 +465,7 @@ pair<VectorValues, Key> 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<GaussianFactorGraph::shared_ptr, VectorValues> QPSolver::constraintsLP( | ||||
|     Key firstSlackKey) const { | ||||
|   // Create constraints and 0 lower bounds (zi>=0)
 | ||||
|  | @ -504,7 +514,7 @@ pair<GaussianFactorGraph::shared_ptr, VectorValues> QPSolver::constraintsLP( | |||
|   return make_pair(constraints, slackLowerBounds); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| //******************************************************************************
 | ||||
| pair<bool, VectorValues> 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<bool, VectorValues> QPSolver::findFeasibleInitialValues() const { | |||
|   return make_pair(slackSum < 1e-5, solution); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| //******************************************************************************
 | ||||
| pair<VectorValues, VectorValues> QPSolver::optimize() const { | ||||
|   bool isFeasible; | ||||
|   VectorValues initials; | ||||
|  |  | |||
|  | @ -23,9 +23,13 @@ namespace gtsam { | |||
|  * and a positive sigma denotes a normal Gaussian noise model. | ||||
|  */ | ||||
| class QPSolver { | ||||
| 
 | ||||
|   class Hessians: public FactorGraph<HessianFactor> { | ||||
|   }; | ||||
| 
 | ||||
|   const GaussianFactorGraph& graph_; //!< the original graph, can't be modified!
 | ||||
|   FastVector<size_t> 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<bool, VectorValues> 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<JacobianFactor>(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<HessianFactor>(factor)); | ||||
|     return hessian; | ||||
|   } | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|   /// Collect all free Hessians involving constrained variables into a graph
 | ||||
|   GaussianFactorGraph::shared_ptr unconstrainedHessiansOfConstrainedVars( | ||||
|       const GaussianFactorGraph& graph, | ||||
|       const std::set<Key>& constrainedVars) const; | ||||
|   void findUnconstrainedHessiansOfConstrainedVars( | ||||
|       const std::set<Key>& constrainedVars); | ||||
| 
 | ||||
| }; | ||||
| 
 | ||||
|  |  | |||
|  | @ -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)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue