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

Fix gtsam's old segfault bug in JacobianFactor::isConstrained: return false if it has no noisemodel. Test Nocedal06book, example 16.4, pg 475 passed.

release/4.3a0
thduynguyen 2014-04-15 17:28:23 -04:00
parent 37079417d1
commit bb7522c947
3 changed files with 49 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/linear/JacobianFactor.h
View File

@ -228,7 +228,7 @@ namespace gtsam {
virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; }
/** is noise model constrained ? */
bool isConstrained() const { return model_->isConstrained(); }
bool isConstrained() const { return model_ && model_->isConstrained(); }
/** Return the dimension of the variable pointed to by the given key iterator
* todo: Remove this in favor of keeping track of dimensions with variables?

7
gtsam/linear/QPSolver.cpp
View File

@ -96,6 +96,8 @@ GaussianFactorGraph::shared_ptr QPSolver::unconstrainedHessiansOfConstrainedVars
/* ************************************************************************* */
GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph,
const VectorValues& x0) const {
static const bool debug = false;
// The dual graph to return
GaussianFactorGraph dualGraph;
@ -110,6 +112,7 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph,
if (xiFactors.size() == 0) continue;
GaussianFactor::shared_ptr xiFactor0 = freeHessians_->at(0);
size_t xiDim = xiFactor0->getDim(xiFactor0->find(xiKey));
if (debug) cout << "xiDim: " << xiDim << endl;
// Compute gradf(xi) = \frac{\partial f}{\partial xi}' = \sum_j G_ij*xj - gi
Vector gradf_xi = zero(xiDim);
@ -146,6 +149,7 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph,
// 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) {
@ -261,6 +265,7 @@ boost::tuple<double, int, int> QPSolver::computeStepSize(const GaussianFactorGra
/* ************************************************************************* */
bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& currentSolution) const {
static bool debug = false;
if (debug) workingGraph.print("workingGraph: ");
// Obtain the solution from the current working graph
VectorValues newSolution = workingGraph.optimize();
if (debug) newSolution.print("New solution:");
@ -268,6 +273,7 @@ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& c
// If we CAN'T move further
if (newSolution.equals(currentSolution, 1e-5)) {
// Compute lambda from the dual graph
if (debug) cout << "Building dual graph..." << endl;
GaussianFactorGraph dualGraph = buildDualGraph(workingGraph, newSolution);
if (debug) dualGraph.print("Dual graph: ");
VectorValues lambdas = dualGraph.optimize();
@ -284,6 +290,7 @@ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& c
else {
// If we CAN make some progress
// Adapt stepsize if some inactive inequality constraints complain about this move
if (debug) cout << "Computing stepsize..." << endl;
double alpha;
int factorIx, sigmaIx;
VectorValues p = newSolution - currentSolution;

48
gtsam/linear/tests/testQPSolver.cpp
View File

@ -136,8 +136,8 @@ TEST(QPSolver, iterate) {
GaussianFactorGraph workingGraph = graph.clone();
VectorValues currentSolution;
currentSolution.insert(X(1), zeros(1,1));
currentSolution.insert(X(2), zeros(1,1));
currentSolution.insert(X(1), zero(1));
currentSolution.insert(X(2), zero(1));
std::vector<VectorValues> expectedSolutions(3);
expectedSolutions[0].insert(X(1), (Vector(1) << 4.0/3.0));
@ -162,8 +162,8 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) {
GaussianFactorGraph graph = createTestCase();
QPSolver solver(graph);
VectorValues initials;
initials.insert(X(1), zeros(1,1));
initials.insert(X(2), zeros(1,1));
initials.insert(X(1), zero(1));
initials.insert(X(2), zero(1));
VectorValues solution = solver.optimize(initials);
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1)<< 1.5));
@ -172,7 +172,7 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) {
}
/* ************************************************************************* */
// Create test graph according to Forst10book_pg171Ex5
// Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html
GaussianFactorGraph createTestMatlabQPEx() {
GaussianFactorGraph graph;
@ -202,8 +202,8 @@ TEST(QPSolver, optimizeMatlabEx) {
GaussianFactorGraph graph = createTestMatlabQPEx();
QPSolver solver(graph);
VectorValues initials;
initials.insert(X(1), zeros(1,1));
initials.insert(X(2), zeros(1,1));
initials.insert(X(1), zero(1));
initials.insert(X(2), zero(1));
VectorValues solution = solver.optimize(initials);
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1)<< 2.0/3.0));
@ -211,6 +211,40 @@ TEST(QPSolver, optimizeMatlabEx) {
CHECK(assert_equal(expectedSolution, solution, 1e-7));
}
/* ************************************************************************* */
// Create test graph as in Nocedal06book, Ex 16.4, pg. 475
GaussianFactorGraph createTestNocedal06bookEx16_4() {
GaussianFactorGraph graph;
graph.push_back(JacobianFactor(X(1), ones(1,1), ones(1)));
graph.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));
return graph;
}
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 solution = solver.optimize(initials);
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1)<< 1.4));
expectedSolution.insert(X(2), (Vector(1)<< 1.7));
CHECK(assert_equal(expectedSolution, solution, 1e-7));
}
/* ************************************************************************* */
int main() {
TestResult tr;