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

Simplified freeHessians_ using inner class

release/4.3a0
dellaert 2014-11-27 10:47:45 +01:00
parent b5e8be56f3
commit 9ca2ba9b66
4 changed files with 1082 additions and 1190 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2148
.cproject
View File

File diff suppressed because it is too large Load Diff

82
gtsam_unstable/linear/QPSolver.cpp
View File

@ -16,7 +16,15 @@ using namespace std;
namespace gtsam { 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) : QPSolver::QPSolver(const GaussianFactorGraph& graph) :
graph_(graph), fullFactorIndices_(graph) { graph_(graph), fullFactorIndices_(graph) {
// Split the original graph into unconstrained and constrained part // 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 // Collect unconstrained hessians of constrained vars to build dual graph
freeHessians_ = unconstrainedHessiansOfConstrainedVars(graph, findUnconstrainedHessiansOfConstrainedVars(constrainedVars);
constrainedVars); freeHessianFactorIndex_ = VariableIndex(freeHessians_);
freeHessianFactorIndex_ = VariableIndex(*freeHessians_);
} }
/* ************************************************************************* */ //******************************************************************************
GaussianFactorGraph::shared_ptr QPSolver::unconstrainedHessiansOfConstrainedVars( void QPSolver::findUnconstrainedHessiansOfConstrainedVars(
const GaussianFactorGraph& graph, const set<Key>& constrainedVars) const { const set<Key>& constrainedVars) {
VariableIndex variableIndex(graph); VariableIndex variableIndex(graph_);
GaussianFactorGraph::shared_ptr hfg(new GaussianFactorGraph());
// Collect all factors involving constrained vars // Collect all factors involving constrained vars
FastSet<size_t> factors; FastSet<size_t> factors;
BOOST_FOREACH(Key key, constrainedVars) { BOOST_FOREACH(Key key, constrainedVars) {
@ -59,10 +66,12 @@ GaussianFactorGraph::shared_ptr QPSolver::unconstrainedHessiansOfConstrainedVars
// Convert each factor into Hessian // Convert each factor into Hessian
BOOST_FOREACH(size_t factorIndex, factors) { BOOST_FOREACH(size_t factorIndex, factors) {
if (!graph[factorIndex]) GaussianFactor::shared_ptr gf = graph_[factorIndex];
if (!gf)
continue; continue;
// See if this is a Jacobian factor // 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) { if (jf) {
// Dealing with mixed constrained factor // Dealing with mixed constrained factor
if (jf->get_model() && jf->isConstrained()) { if (jf->get_model() && jf->isConstrained()) {
@ -82,7 +91,7 @@ GaussianFactorGraph::shared_ptr QPSolver::unconstrainedHessiansOfConstrainedVars
JacobianFactor::shared_ptr newJacobian = toJacobian(jf->clone()); JacobianFactor::shared_ptr newJacobian = toJacobian(jf->clone());
newJacobian->setModel( newJacobian->setModel(
noiseModel::Diagonal::Precisions(newPrecisions)); noiseModel::Diagonal::Precisions(newPrecisions));
hfg->push_back(HessianFactor(*newJacobian)); freeHessians_.push_back(HessianFactor(*newJacobian));
} }
} else { // unconstrained Jacobian } else { // unconstrained Jacobian
// Convert the original linear factor to Hessian factor // 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 // because of a weird error which might be related to clang
// See this: https://groups.google.com/forum/#!topic/ceres-solver/DYhqOLPquHU // 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 // 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! } 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, GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph,
const VectorValues& x0, bool useLeastSquare) const { const VectorValues& x0, bool useLeastSquare) const {
static const bool debug = false; 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 // Find xi's dim from the first factor on xi
if (xiFactors.size() == 0) if (xiFactors.size() == 0)
continue; continue;
GaussianFactor::shared_ptr xiFactor0 = freeHessians_->at( GaussianFactor::shared_ptr xiFactor0 = freeHessians_.at(*xiFactors.begin());
*xiFactors.begin());
size_t xiDim = xiFactor0->getDim(xiFactor0->find(xiKey)); size_t xiDim = xiFactor0->getDim(xiFactor0->find(xiKey));
if (debug) if (debug)
xiFactor0->print("xiFactor0: "); xiFactor0->print("xiFactor0: ");
@ -131,12 +141,12 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph,
cout << "xiKey: " << string(Symbol(xiKey)) << ", xiDim: " << xiDim cout << "xiKey: " << string(Symbol(xiKey)) << ", xiDim: " << xiDim
<< endl; << endl;
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Compute the b-vector for the dual factor Ax-b // Compute the b-vector for the dual factor Ax-b
// b = gradf(xi) = \frac{\partial f}{\partial xi}' = \sum_j G_ij*xj - gi // b = gradf(xi) = \frac{\partial f}{\partial xi}' = \sum_j G_ij*xj - gi
Vector gradf_xi = zero(xiDim); Vector gradf_xi = zero(xiDim);
BOOST_FOREACH(size_t factorIx, xiFactors) { 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); Factor::const_iterator xi = factor->find(xiKey);
// Sum over Gij*xj for all xj connecting to xi // Sum over Gij*xj for all xj connecting to xi
for (Factor::const_iterator xj = factor->begin(); xj != factor->end(); 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); gradf_xi += -factor->linearTerm(xi);
} }
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Compute the Jacobian A for the dual factor Ax-b // Compute the Jacobian A for the dual factor Ax-b
// Obtain the jacobians for lambda variables from their corresponding constraints // Obtain the jacobians for lambda variables from their corresponding constraints
// A = gradc_k(xi) = \frac{\partial c_k}{\partial xi}' // 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)); lambdaTerms.push_back(make_pair(factorIndex, A_k));
} }
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Create and add factors to the dual graph // Create and add factors to the dual graph
// If least square approximation is desired, use unit noise model. // If least square approximation is desired, use unit noise model.
if (debug) if (debug)
@ -232,7 +242,7 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph,
return dualGraph; return dualGraph;
} }
/* ************************************************************************* */ //******************************************************************************
pair<int, int> QPSolver::findWorstViolatedActiveIneq( pair<int, int> QPSolver::findWorstViolatedActiveIneq(
const VectorValues& lambdas) const { const VectorValues& lambdas) const {
int worstFactorIx = -1, worstSigmaIx = -1; int worstFactorIx = -1, worstSigmaIx = -1;
@ -253,9 +263,9 @@ pair<int, int> QPSolver::findWorstViolatedActiveIneq(
return make_pair(worstFactorIx, worstSigmaIx); return make_pair(worstFactorIx, worstSigmaIx);
} }
/* ************************************************************************* */bool QPSolver::updateWorkingSetInplace( //******************************************************************************
GaussianFactorGraph& workingGraph, int factorIx, int sigmaIx, bool QPSolver::updateWorkingSetInplace(GaussianFactorGraph& workingGraph,
double newSigma) const { int factorIx, int sigmaIx, double newSigma) const {
if (factorIx < 0 || sigmaIx < 0) if (factorIx < 0 || sigmaIx < 0)
return false; return false;
Vector sigmas = toJacobian(workingGraph.at(factorIx))->get_model()->sigmas(); Vector sigmas = toJacobian(workingGraph.at(factorIx))->get_model()->sigmas();
@ -264,7 +274,7 @@ pair<int, int> QPSolver::findWorstViolatedActiveIneq(
return true; return true;
} }
/* ************************************************************************* */ //******************************************************************************
/* We have to make sure the new solution with alpha satisfies all INACTIVE ineq constraints /* 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), * 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 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); return boost::make_tuple(minAlpha, closestFactorIx, closestSigmaIx);
} }
/* ************************************************************************* */bool QPSolver::iterateInPlace( //******************************************************************************
GaussianFactorGraph& workingGraph, VectorValues& currentSolution, bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph,
VectorValues& lambdas) const { VectorValues& currentSolution, VectorValues& lambdas) const {
static bool debug = false; static bool debug = false;
if (debug) if (debug)
workingGraph.print("workingGraph: "); workingGraph.print("workingGraph: ");
@ -400,7 +410,7 @@ boost::tuple<double, int, int> QPSolver::computeStepSize(
return false; return false;
} }
/* ************************************************************************* */ //******************************************************************************
pair<VectorValues, VectorValues> QPSolver::optimize( pair<VectorValues, VectorValues> QPSolver::optimize(
const VectorValues& initials) const { const VectorValues& initials) const {
GaussianFactorGraph workingGraph = graph_.clone(); GaussianFactorGraph workingGraph = graph_.clone();
@ -413,7 +423,7 @@ pair<VectorValues, VectorValues> QPSolver::optimize(
return make_pair(currentSolution, lambdas); return make_pair(currentSolution, lambdas);
} }
/* ************************************************************************* */ //******************************************************************************
pair<VectorValues, Key> QPSolver::initialValuesLP() const { pair<VectorValues, Key> QPSolver::initialValuesLP() const {
size_t firstSlackKey = 0; size_t firstSlackKey = 0;
BOOST_FOREACH(Key key, fullFactorIndices_ | boost::adaptors::map_keys) { BOOST_FOREACH(Key key, fullFactorIndices_ | boost::adaptors::map_keys) {
@ -455,7 +465,7 @@ pair<VectorValues, Key> QPSolver::initialValuesLP() const {
return make_pair(initials, firstSlackKey); return make_pair(initials, firstSlackKey);
} }
/* ************************************************************************* */ //******************************************************************************
VectorValues QPSolver::objectiveCoeffsLP(Key firstSlackKey) const { VectorValues QPSolver::objectiveCoeffsLP(Key firstSlackKey) const {
VectorValues slackObjective; VectorValues slackObjective;
for (size_t i = 0; i < constraintIndices_.size(); ++i) { for (size_t i = 0; i < constraintIndices_.size(); ++i) {
@ -474,7 +484,7 @@ VectorValues QPSolver::objectiveCoeffsLP(Key firstSlackKey) const {
return slackObjective; return slackObjective;
} }
/* ************************************************************************* */ //******************************************************************************
pair<GaussianFactorGraph::shared_ptr, VectorValues> QPSolver::constraintsLP( pair<GaussianFactorGraph::shared_ptr, VectorValues> QPSolver::constraintsLP(
Key firstSlackKey) const { Key firstSlackKey) const {
// Create constraints and 0 lower bounds (zi>=0) // Create constraints and 0 lower bounds (zi>=0)
@ -504,7 +514,7 @@ pair<GaussianFactorGraph::shared_ptr, VectorValues> QPSolver::constraintsLP(
return make_pair(constraints, slackLowerBounds); return make_pair(constraints, slackLowerBounds);
} }
/* ************************************************************************* */ //******************************************************************************
pair<bool, VectorValues> QPSolver::findFeasibleInitialValues() const { pair<bool, VectorValues> QPSolver::findFeasibleInitialValues() const {
static const bool debug = false; static const bool debug = false;
// Initial values with slack variables for the LP subproblem, Nocedal06book, pg.473 // Initial values with slack variables for the LP subproblem, Nocedal06book, pg.473
@ -554,7 +564,7 @@ pair<bool, VectorValues> QPSolver::findFeasibleInitialValues() const {
return make_pair(slackSum < 1e-5, solution); return make_pair(slackSum < 1e-5, solution);
} }
/* ************************************************************************* */ //******************************************************************************
pair<VectorValues, VectorValues> QPSolver::optimize() const { pair<VectorValues, VectorValues> QPSolver::optimize() const {
bool isFeasible; bool isFeasible;
VectorValues initials; VectorValues initials;

32
gtsam_unstable/linear/QPSolver.h
View File

@ -23,9 +23,13 @@ namespace gtsam {
* and a positive sigma denotes a normal Gaussian noise model. * and a positive sigma denotes a normal Gaussian noise model.
*/ */
class QPSolver { class QPSolver {
class Hessians: public FactorGraph<HessianFactor> {
};
const GaussianFactorGraph& graph_; //!< the original graph, can't be modified! const GaussianFactorGraph& graph_; //!< the original graph, can't be modified!
FastVector<size_t> constraintIndices_; //!< Indices of constrained factors in the original graph 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 VariableIndex freeHessianFactorIndex_; //!< indices of unconstrained Hessian factors of constrained variables
// gtsam calls it "VariableIndex", but I think FactorIndex // gtsam calls it "VariableIndex", but I think FactorIndex
// makes more sense, because it really stores factor indices. // makes more sense, because it really stores factor indices.
@ -43,7 +47,7 @@ public:
} }
/// Return the Hessian factor graph of constrained variables /// Return the Hessian factor graph of constrained variables
GaussianFactorGraph::shared_ptr freeHessiansOfConstrainedVars() const { const Hessians& freeHessiansOfConstrainedVars() const {
return freeHessians_; return freeHessians_;
} }
@ -172,29 +176,11 @@ public:
/// Find a feasible initial point /// Find a feasible initial point
std::pair<bool, VectorValues> findFeasibleInitialValues() const; 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: private:
/// Collect all free Hessians involving constrained variables into a graph /// Collect all free Hessians involving constrained variables into a graph
GaussianFactorGraph::shared_ptr unconstrainedHessiansOfConstrainedVars( void findUnconstrainedHessiansOfConstrainedVars(
const GaussianFactorGraph& graph, const std::set<Key>& constrainedVars);
const std::set<Key>& constrainedVars) const;
}; };

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

@ -89,13 +89,9 @@ TEST(QPSolver, constraintsAux) {
LONGS_EQUAL(-1, factorIx2); LONGS_EQUAL(-1, factorIx2);
LONGS_EQUAL(-1, lambdaIx2); LONGS_EQUAL(-1, lambdaIx2);
GaussianFactorGraph::shared_ptr freeHessian = HessianFactor expectedFreeHessian(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1),
solver.freeHessiansOfConstrainedVars(); 3.0 * ones(1), 2.0 * ones(1, 1), zero(1), 1.0);
GaussianFactorGraph expectedFreeHessian; EXPECT(solver.freeHessiansOfConstrainedVars()[0]->equals(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));
} }
/* ************************************************************************* */ /* ************************************************************************* */