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

Fix parser to initialize values correctly.

release/4.3a0
= 2018-11-04 13:10:14 -05:00
parent 1ee4f5e8d9
commit b851c498b7
2 changed files with 20 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam_unstable/linear/RawQP.cpp
View File

@ -244,17 +244,23 @@ QP RawQP::makeQP() {
keys.push_back(kv.second); keys.push_back(kv.second);
} }
std::sort(keys.begin(), keys.end()); std::sort(keys.begin(), keys.end());
Matrix11 G_value;
for (unsigned int i = 0; i < keys.size(); ++i) { for (unsigned int i = 0; i < keys.size(); ++i) {
for (unsigned int j = i; j < keys.size(); ++j) { for (unsigned int j = i; j < keys.size(); ++j) {
G_value = H[keys[i]][keys[j]]; if (H.count(keys[i]) > 0 and H[keys[i]].count(keys[j]) > 0){
Gs.push_back(G_value.hasNaN() ? Z_1x1 : G_value); Gs.emplace_back(H[keys[i]][keys[j]]);
}
else{
Gs.emplace_back(Z_1x1);
}
} }
} }
Matrix11 g_value;
for (Key key1 : keys) { for (Key key1 : keys) {
g_value = -g[key1]; if(g.count(key1) > 0){
gs.push_back(g_value.hasNaN() ? Z_1x1 : g_value); gs.emplace_back(-g[key1]);
}
else{
gs.emplace_back(Z_1x1);
}
} }
size_t dual_key_num = keys.size() + 1; size_t dual_key_num = keys.size() + 1;
QP madeQP; QP madeQP;

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

@ -13,7 +13,7 @@
* @file testQPSolver.cpp * @file testQPSolver.cpp
* @brief Test simple QP solver for a linear inequality constraint * @brief Test simple QP solver for a linear inequality constraint
* @date Apr 10, 2014 * @date Apr 10, 2014
* @author Duy-Nguyen Ta * @author Duy-Nguyen Ta, Ivan Dario Jimenez
*/ */
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
@ -281,7 +281,7 @@ TEST(QPSolver, HS21) {
CHECK(assert_equal(expectedSolution, actualSolution)) CHECK(assert_equal(expectedSolution, actualSolution))
} }
TEST_DISABLED(QPSolver, HS118) { //Fails because of the GTSAM linear system error TEST_DISABLED(QPSolver, HS118) { // TOO LARGE
QP problem = QPSParser("HS118.QPS").Parse(); QP problem = QPSParser("HS118.QPS").Parse();
VectorValues actualSolution; VectorValues actualSolution;
VectorValues expectedSolution; VectorValues expectedSolution;
@ -319,7 +319,7 @@ TEST(QPSolver, HS51) {
CHECK(assert_equal(8.88178420e-16,error_actual, 1e-7)) CHECK(assert_equal(8.88178420e-16,error_actual, 1e-7))
} }
TEST(QPSolver, HS52) { //Fails on release but not debug TEST(QPSolver, HS52) {
QP problem = QPSParser("HS52.QPS").Parse(); QP problem = QPSParser("HS52.QPS").Parse();
VectorValues actualSolution; VectorValues actualSolution;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
@ -327,7 +327,7 @@ TEST(QPSolver, HS52) { //Fails on release but not debug
CHECK(assert_equal(5.32664756,error_actual, 1e-7)) CHECK(assert_equal(5.32664756,error_actual, 1e-7))
} }
TEST_DISABLED(QPSolver, HS53) { //Fails because of the GTSAM indeterminant linear system error TEST_DISABLED(QPSolver, HS53) { // TOO LARGE
QP problem = QPSParser("HS53.QPS").Parse(); QP problem = QPSParser("HS53.QPS").Parse();
VectorValues actualSolution; VectorValues actualSolution;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
@ -335,7 +335,7 @@ TEST_DISABLED(QPSolver, HS53) { //Fails because of the GTSAM indeterminant linea
CHECK(assert_equal(4.09302326,error_actual, 1e-7)) CHECK(assert_equal(4.09302326,error_actual, 1e-7))
} }
TEST_DISABLED(QPSolver, HS76) { //Fails because of the GTSAM indeterminant linear system TEST_DISABLED(QPSolver, HS76) { //TOO LARGE
QP problem = QPSParser("HS76.QPS").Parse(); QP problem = QPSParser("HS76.QPS").Parse();
VectorValues actualSolution; VectorValues actualSolution;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
@ -351,7 +351,7 @@ TEST(QPSolver, HS268) { // This test needs an extra order of magnitude of tolera
CHECK(assert_equal(5.73107049e-07,error_actual, 1e-6)) CHECK(assert_equal(5.73107049e-07,error_actual, 1e-6))
} }
TEST_DISABLED(QPSolver, AUG2D) { //Fails with Indeterminant Linear System error. TEST_DISABLED(QPSolver, AUG2D) { //TOO LARGE
QP problem = QPSParser("AUG2D.QPS").Parse(); QP problem = QPSParser("AUG2D.QPS").Parse();
VectorValues actualSolution; VectorValues actualSolution;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
@ -359,7 +359,7 @@ TEST_DISABLED(QPSolver, AUG2D) { //Fails with Indeterminant Linear System error.
CHECK(assert_equal(0.168741175e+07,error_actual, 1e-7)) CHECK(assert_equal(0.168741175e+07,error_actual, 1e-7))
} }
TEST_DISABLED(QPSolver, CONT_050) { // Fails with Indeterminant Linear System error TEST_DISABLED(QPSolver, CONT_050) { //TOO LARGE
QP problem = QPSParser("CONT-050.QPS").Parse(); QP problem = QPSParser("CONT-050.QPS").Parse();
VectorValues actualSolution; VectorValues actualSolution;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
@ -367,11 +367,8 @@ TEST_DISABLED(QPSolver, CONT_050) { // Fails with Indeterminant Linear System er
CHECK(assert_equal(-4.56385090,error_actual, 1e-7)) CHECK(assert_equal(-4.56385090,error_actual, 1e-7))
} }
TEST_DISABLED(QPSolver, QPTEST) { // Fails with Indeterminant Linear System error TEST_DISABLED(QPSolver, QPTEST) { // REQUIRES Jacobian Fix
QP problem = QPSParser("QPTEST.QPS").Parse(); QP problem = QPSParser("QPTEST.QPS").Parse();
GTSAM_PRINT(problem.cost);
GTSAM_PRINT(problem.equalities);
GTSAM_PRINT(problem.inequalities);
VectorValues actualSolution; VectorValues actualSolution;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
double error_actual = problem.cost.error(actualSolution); double error_actual = problem.cost.error(actualSolution);