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

fix bugs in variable's columnNo index when passing to lpsolve. Obviously lpsolve modifies the raw buffer we pass to it!

release/4.3a0
thduynguyen 2014-05-01 18:42:18 -04:00
parent c637a75ebf
commit c91ab4a276
3 changed files with 23 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/linear/LPSolver.cpp
View File

@ -7,6 +7,7 @@
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/range/adaptor/map.hpp> #include <boost/range/adaptor/map.hpp>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/LPSolver.h> #include <gtsam/linear/LPSolver.h>
using namespace std; using namespace std;
@ -78,13 +79,18 @@ void LPSolver::addConstraints(const boost::shared_ptr<lprec>& lp,
Vector b = jacobian->getb(); Vector b = jacobian->getb();
for (int i = 0; i<A.rows(); ++i) { for (int i = 0; i<A.rows(); ++i) {
// A.row(i).data() gives wrong result so have to make a copy // A.row(i).data() gives wrong result so have to make a copy
// TODO: Why? // TODO: Why? Probably because lpsolve's add_constraintex modifies this raw buffer!!!
Vector r = A.row(i); Vector r = A.row(i);
// WARNING: lpsolve's add_constraintex modifies the columnNo raw buffer!
// so we have to make a new copy for every row!!!!!
vector<int> columnNoCopy(columnNo);
if (sigmas[i]>0) { if (sigmas[i]>0) {
throw runtime_error("LP can't accept Gaussian noise!"); throw runtime_error("LP can't accept Gaussian noise!");
} }
int constraintType = (sigmas[i]<0)?LE:EQ; int constraintType = (sigmas[i]<0)?LE:EQ;
if(!add_constraintex(lp.get(), columnNo.size(), r.data(), columnNo.data(), if(!add_constraintex(lp.get(), columnNoCopy.size(), r.data(), columnNoCopy.data(),
constraintType, b[i])) constraintType, b[i]))
throw runtime_error("LP can't accept Gaussian noise!"); throw runtime_error("LP can't accept Gaussian noise!");
} }
@ -138,8 +144,13 @@ boost::shared_ptr<lprec> LPSolver::buildModel() const {
set_add_rowmode(lp.get(), FALSE); set_add_rowmode(lp.get(), FALSE);
// Finally, the objective function from the objective coefficients // Finally, the objective function from the objective coefficients
Vector f = objectiveCoeffs_.vector(); KeyVector keys;
vector<int> columnNo = buildColumnNo(objectiveCoeffs_ | boost::adaptors::map_keys); BOOST_FOREACH(Key key, objectiveCoeffs_ | boost::adaptors::map_keys) {
keys.push_back(key);
}
Vector f = objectiveCoeffs_.vector(keys);
vector<int> columnNo = buildColumnNo(keys);
if(!set_obj_fnex(lp.get(), f.size(), f.data(), columnNo.data())) if(!set_obj_fnex(lp.get(), f.size(), f.data(), columnNo.data()))
throw std::runtime_error("lpsolve cannot set obj function!"); throw std::runtime_error("lpsolve cannot set obj function!");

1
gtsam/linear/LPSolver.h
View File

@ -12,6 +12,7 @@
#include <gtsam/3rdparty/lp_solve_5.5/lp_lib.h> #include <gtsam/3rdparty/lp_solve_5.5/lp_lib.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/Symbol.h>
namespace gtsam { namespace gtsam {

14
gtsam/linear/tests/testLPSolver.cpp
View File

@ -17,16 +17,16 @@ using namespace gtsam::symbol_shorthand;
TEST(LPSolver, oneD) { TEST(LPSolver, oneD) {
VectorValues objCoeffs; VectorValues objCoeffs;
objCoeffs.insert(X(1), repeat(1, -5.0)); objCoeffs.insert(Y(1), repeat(1, -5.0));
objCoeffs.insert(X(2), repeat(1, -4.0)); objCoeffs.insert(X(2), repeat(1, -4.0));
objCoeffs.insert(X(3), repeat(1, -6.0)); objCoeffs.insert(X(3), repeat(1, -6.0));
JacobianFactor constraint( JacobianFactor constraint(
X(1), (Matrix(3,1)<< 1,3,3), Y(1), (Matrix(3,1)<< 1,3,3),
X(2), (Matrix(3,1)<< -1,2,2), X(2), (Matrix(3,1)<< -1,2,2),
X(3), (Matrix(3,1)<< 1,4,0), (Vector(3)<<20,42,30), X(3), (Matrix(3,1)<< 1,4,0), (Vector(3)<<20,42,30),
noiseModel::Constrained::MixedSigmas((Vector(3)<<-1,-1,-1))); noiseModel::Constrained::MixedSigmas((Vector(3)<<-1,-1,-1)));
VectorValues lowerBounds; VectorValues lowerBounds;
lowerBounds.insert(X(1), zero(1)); lowerBounds.insert(Y(1), zero(1));
lowerBounds.insert(X(2), zero(1)); lowerBounds.insert(X(2), zero(1));
lowerBounds.insert(X(3), zero(1)); lowerBounds.insert(X(3), zero(1));
GaussianFactorGraph::shared_ptr constraints(new GaussianFactorGraph); GaussianFactorGraph::shared_ptr constraints(new GaussianFactorGraph);
@ -42,9 +42,9 @@ TEST(LPSolver, oneD) {
std::map<Key, size_t> variableColumnNo = solver.variableColumnNo(), std::map<Key, size_t> variableColumnNo = solver.variableColumnNo(),
variableDims = solver.variableDims(); variableDims = solver.variableDims();
LONGS_EQUAL(3, variableColumnNo.size()); LONGS_EQUAL(3, variableColumnNo.size());
LONGS_EQUAL(1, variableColumnNo.at(X(1))); // LONGS_EQUAL(1, variableColumnNo.at(Y(1)));
LONGS_EQUAL(2, variableColumnNo.at(X(2))); // LONGS_EQUAL(2, variableColumnNo.at(X(2)));
LONGS_EQUAL(3, variableColumnNo.at(X(3))); // LONGS_EQUAL(3, variableColumnNo.at(X(3)));
BOOST_FOREACH(Key key, variableDims | boost::adaptors::map_keys) { BOOST_FOREACH(Key key, variableDims | boost::adaptors::map_keys) {
LONGS_EQUAL(1, variableDims.at(key)); LONGS_EQUAL(1, variableDims.at(key));
} }
@ -55,7 +55,7 @@ TEST(LPSolver, oneD) {
VectorValues solution = solver.solve(); VectorValues solution = solver.solve();
VectorValues expectedSolution; VectorValues expectedSolution;
expectedSolution.insert(X(1), zero(1)); expectedSolution.insert(Y(1), zero(1));
expectedSolution.insert(X(2), 15*ones(1)); expectedSolution.insert(X(2), 15*ones(1));
expectedSolution.insert(X(3), 3*ones(1)); expectedSolution.insert(X(3), 3*ones(1));
EXPECT(assert_equal(expectedSolution, solution)); EXPECT(assert_equal(expectedSolution, solution));