Skip to content

Commit

Permalink
Merged in feature/LPSolver (pull request #277)
Browse files Browse the repository at this point in the history 
Long awaited fixe of some QPSolver tests
Feature/lpsolver
  • Loading branch information
@ivandariojr @dellaert
ivandariojr authored and dellaert committed Apr 27, 2019
2 parents 9422586 + 5f4cede commit a5844cb
Show file tree
Hide file tree
Showing 14 changed files with 767 additions and 460 deletions.
20 changes: 20 additions & 0 deletions examples/Data/HS21.QPS
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
NAME HS21
ROWS
N OBJ.FUNC
G R------1
COLUMNS
C------1 R------1 0.100000e+02
C------2 R------1 -.100000e+01
RHS
RHS OBJ.FUNC 0.100000e+03
RHS R------1 0.100000e+02
RANGES
BOUNDS
LO BOUNDS C------1 0.200000e+01
UP BOUNDS C------1 0.500000e+02
LO BOUNDS C------2 -.500000e+02
UP BOUNDS C------2 0.500000e+02
QUADOBJ
C------1 C------1 0.200000e-01
C------2 C------2 0.200000e+01
ENDATA
55 changes: 55 additions & 0 deletions examples/Data/HS268.QPS
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,55 @@
NAME HS268
ROWS
N OBJ.FUNC
G R------1
G R------2
G R------3
G R------4
G R------5
COLUMNS
C------1 OBJ.FUNC 0.183400e+05 R------1 -.100000e+01
C------1 R------2 0.100000e+02 R------3 -.800000e+01
C------1 R------4 0.800000e+01 R------5 -.400000e+01
C------2 OBJ.FUNC -.341980e+05 R------1 -.100000e+01
C------2 R------2 0.100000e+02 R------3 0.100000e+01
C------2 R------4 -.100000e+01 R------5 -.200000e+01
C------3 OBJ.FUNC 0.454200e+04 R------1 -.100000e+01
C------3 R------2 -.300000e+01 R------3 -.200000e+01
C------3 R------4 0.200000e+01 R------5 0.300000e+01
C------4 OBJ.FUNC 0.867200e+04 R------1 -.100000e+01
C------4 R------2 0.500000e+01 R------3 -.500000e+01
C------4 R------4 0.500000e+01 R------5 -.500000e+01
C------5 OBJ.FUNC 0.860000e+02 R------1 -.100000e+01
C------5 R------2 0.400000e+01 R------3 0.300000e+01
C------5 R------4 -.300000e+01 R------5 0.100000e+01
RHS
RHS OBJ.FUNC -.144630e+05
RHS R------1 -.500000e+01
RHS R------2 0.200000e+02
RHS R------3 -.400000e+02
RHS R------4 0.110000e+02
RHS R------5 -.300000e+02
RANGES
BOUNDS
FR BOUNDS C------1
FR BOUNDS C------2
FR BOUNDS C------3
FR BOUNDS C------4
FR BOUNDS C------5
QUADOBJ
C------1 C------1 0.203940e+05
C------1 C------2 -.249080e+05
C------1 C------3 -.202600e+04
C------1 C------4 0.389600e+04
C------1 C------5 0.658000e+03
C------2 C------2 0.418180e+05
C------2 C------3 -.346600e+04
C------2 C------4 -.982800e+04
C------2 C------5 -.372000e+03
C------3 C------3 0.351000e+04
C------3 C------4 0.217800e+04
C------3 C------5 -.348000e+03
C------4 C------4 0.303000e+04
C------4 C------5 -.440000e+02
C------5 C------5 0.540000e+02
ENDATA
20 changes: 20 additions & 0 deletions examples/Data/HS35.QPS
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
NAME HS35
ROWS
N OBJ.FUNC
G R------1
COLUMNS
C------1 OBJ.FUNC -.800000e+01 R------1 -.100000e+01
C------2 OBJ.FUNC -.600000e+01 R------1 -.100000e+01
C------3 OBJ.FUNC -.400000e+01 R------1 -.200000e+01
RHS
RHS OBJ.FUNC -.900000e+01
RHS R------1 -.300000e+01
RANGES
BOUNDS
QUADOBJ
C------1 C------1 0.400000e+01
C------1 C------2 0.200000e+01
C------1 C------3 0.200000e+01
C------2 C------2 0.400000e+01
C------3 C------3 0.200000e+01
ENDATA
21 changes: 21 additions & 0 deletions examples/Data/HS35MOD.QPS
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
NAME HS35MOD
ROWS
N OBJ.FUNC
G R------1
COLUMNS
C------1 OBJ.FUNC -.800000e+01 R------1 -.100000e+01
C------2 OBJ.FUNC -.600000e+01 R------1 -.100000e+01
C------3 OBJ.FUNC -.400000e+01 R------1 -.200000e+01
RHS
RHS OBJ.FUNC -.900000e+01
RHS R------1 -.300000e+01
RANGES
BOUNDS
FX BOUNDS C------2 0.500000e+00
QUADOBJ
C------1 C------1 0.400000e+01
C------1 C------2 0.200000e+01
C------1 C------3 0.200000e+01
C------2 C------2 0.400000e+01
C------3 C------3 0.200000e+01
ENDATA
33 changes: 33 additions & 0 deletions examples/Data/HS51.QPS
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
NAME HS51
ROWS
N OBJ.FUNC
E R------1
E R------2
E R------3
COLUMNS
C------1 R------1 0.100000e+01
C------2 OBJ.FUNC -.400000e+01 R------1 0.300000e+01
C------2 R------3 0.100000e+01
C------3 OBJ.FUNC -.400000e+01 R------2 0.100000e+01
C------4 OBJ.FUNC -.200000e+01 R------2 0.100000e+01
C------5 OBJ.FUNC -.200000e+01 R------2 -.200000e+01
C------5 R------3 -.100000e+01
RHS
RHS OBJ.FUNC -.600000e+01
RHS R------1 0.400000e+01
RANGES
BOUNDS
FR BOUNDS C------1
FR BOUNDS C------2
FR BOUNDS C------3
FR BOUNDS C------4
FR BOUNDS C------5
QUADOBJ
C------1 C------1 0.200000e+01
C------1 C------2 -.200000e+01
C------2 C------2 0.400000e+01
C------2 C------3 0.200000e+01
C------3 C------3 0.200000e+01
C------4 C------4 0.200000e+01
C------5 C------5 0.200000e+01
ENDATA
32 changes: 32 additions & 0 deletions examples/Data/HS52.QPS
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
NAME HS52
ROWS
N OBJ.FUNC
E R------1
E R------2
E R------3
COLUMNS
C------1 R------1 0.100000e+01
C------2 OBJ.FUNC -.400000e+01 R------1 0.300000e+01
C------2 R------3 0.100000e+01
C------3 OBJ.FUNC -.400000e+01 R------2 0.100000e+01
C------4 OBJ.FUNC -.200000e+01 R------2 0.100000e+01
C------5 OBJ.FUNC -.200000e+01 R------2 -.200000e+01
C------5 R------3 -.100000e+01
RHS
RHS OBJ.FUNC -.600000e+01
RANGES
BOUNDS
FR BOUNDS C------1
FR BOUNDS C------2
FR BOUNDS C------3
FR BOUNDS C------4
FR BOUNDS C------5
QUADOBJ
C------1 C------1 0.320000e+02
C------1 C------2 -.800000e+01
C------2 C------2 0.400000e+01
C------2 C------3 0.200000e+01
C------3 C------3 0.200000e+01
C------4 C------4 0.200000e+01
C------5 C------5 0.200000e+01
ENDATA
19 changes: 19 additions & 0 deletions examples/Data/QPTEST.QPS
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
NAME QP example
ROWS
N obj
G r1
L r2
COLUMNS
c1 r1 2.0 r2 -1.0
c1 obj 1.5
c2 r1 1.0 r2 2.0
c2 obj -2.0
RHS
rhs1 r1 2.0 r2 6.0
BOUNDS
UP bnd1 c1 20.0
QUADOBJ
c1 c1 8.0
c1 c2 2.0
c2 c2 10.0
ENDATA
28 changes: 15 additions & 13 deletions gtsam/linear/JacobianFactor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,10 +102,9 @@ JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2,
}

/* ************************************************************************* */
JacobianFactor::JacobianFactor(const HessianFactor& factor) :
Base(factor), Ab_(
VerticalBlockMatrix::LikeActiveViewOf(factor.info(),
factor.rows())) {
JacobianFactor::JacobianFactor(const HessianFactor& factor)
: Base(factor),
Ab_(VerticalBlockMatrix::LikeActiveViewOf(factor.info(), factor.rows())) {
// Copy Hessian into our matrix and then do in-place Cholesky
Ab_.full() = factor.info().selfadjointView();

Expand All @@ -114,16 +113,19 @@ JacobianFactor::JacobianFactor(const HessianFactor& factor) :
bool success;
boost::tie(maxrank, success) = choleskyCareful(Ab_.matrix());

// Check for indefinite system
if (!success)
// Check that Cholesky succeeded OR it managed to factor the full Hessian.
// THe latter case occurs with non-positive definite matrices arising from QP.
if (success || maxrank == factor.rows() - 1) {
// Zero out lower triangle
Ab_.matrix().topRows(maxrank).triangularView<Eigen::StrictlyLower>() =
Matrix::Zero(maxrank, Ab_.matrix().cols());
// FIXME: replace with triangular system
Ab_.rowEnd() = maxrank;
model_ = SharedDiagonal(); // is equivalent to Unit::Create(maxrank)
} else {
// indefinite system
throw IndeterminantLinearSystemException(factor.keys().front());

// Zero out lower triangle
Ab_.matrix().topRows(maxrank).triangularView<Eigen::StrictlyLower>() =
Matrix::Zero(maxrank, Ab_.matrix().cols());
// FIXME: replace with triangular system
Ab_.rowEnd() = maxrank;
model_ = SharedDiagonal(); // should be same as Unit::Create(maxrank);
}
}

/* ************************************************************************* */
Expand Down
25 changes: 23 additions & 2 deletions gtsam/linear/tests/testJacobianFactor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,22 +146,43 @@ TEST(JacobianFactor, constructors_and_accessors)

/* ************************************************************************* */
TEST(JabobianFactor, Hessian_conversion) {
HessianFactor hessian(0, (Matrix(4,4) <<
HessianFactor hessian(0, (Matrix(4, 4) <<
1.57, 2.695, -1.1, -2.35,
2.695, 11.3125, -0.65, -10.225,
-1.1, -0.65, 1, 0.5,
-2.35, -10.225, 0.5, 9.25).finished(),
(Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(),
73.1725);

JacobianFactor expected(0, (Matrix(2,4) <<
JacobianFactor expected(0, (Matrix(2, 4) <<
1.2530, 2.1508, -0.8779, -1.8755,
0, 2.5858, 0.4789, -2.3943).finished(),
Vector2(-6.2929, -5.7941));

EXPECT(assert_equal(expected, JacobianFactor(hessian), 1e-3));
}

/* ************************************************************************* */
TEST(JabobianFactor, Hessian_conversion2) {
JacobianFactor jf(0, (Matrix(3, 3) <<
1, 2, 3,
0, 2, 3,
0, 0, 3).finished(),
Vector3(1, 2, 2));
HessianFactor hessian(jf);
EXPECT(assert_equal(jf, JacobianFactor(hessian), 1e-9));
}

/* ************************************************************************* */
TEST(JabobianFactor, Hessian_conversion3) {
JacobianFactor jf(0, (Matrix(2, 4) <<
1, 2, 3, 0,
0, 3, 2, 1).finished(),
Vector2(1, 2));
HessianFactor hessian(jf);
EXPECT(assert_equal(jf, JacobianFactor(hessian), 1e-9));
}

/* ************************************************************************* */
namespace simple_graph {

Expand Down
Loading

0 comments on commit a5844cb

Please sign in to comment.