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

[BUGFIX] Now handles zero default on QPS files.

release/4.3a0
= 2016-06-28 20:39:36 -04:00
parent c9efb4bbda
commit ab045edf07
1 changed files with 11 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam_unstable/linear/RawQP.cpp
View File

@ -210,22 +210,26 @@ 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) {
Gs.push_back(H[keys[i]][keys[j]]); G_value = H[keys[i]][keys[j]];
Gs.push_back(G_value.hasNaN() ? Z_1x1 : G_value);
} }
} }
Matrix11 g_value;
for (Key key1 : keys) { for (Key key1 : keys) {
gs.push_back(-g[key1]); g_value = -g[key1];
gs.push_back(g_value.hasNaN() ? Z_1x1 : g_value);
} }
int dual_key_num = keys.size() + 1; int dual_key_num = keys.size() + 1;
QP madeQP; QP madeQP;
auto obj = HessianFactor(keys, Gs, gs, f); auto obj = HessianFactor(keys, Gs, gs, 2 * f);
madeQP.cost.push_back(obj); madeQP.cost.push_back(obj);
for (auto kv : E) { for (auto kv : E) {
std::map<Key, Matrix11> keyMatrixMap; std::map < Key, Matrix11 > keyMatrixMap;
for (auto km : kv.second) { for (auto km : kv.second) {
keyMatrixMap.insert(km); keyMatrixMap.insert(km);
} }
@ -234,7 +238,7 @@ QP RawQP::makeQP() {
} }
for (auto kv : IG) { for (auto kv : IG) {
std::map<Key, Matrix11> keyMatrixMap; std::map < Key, Matrix11 > keyMatrixMap;
for (auto km : kv.second) { for (auto km : kv.second) {
km.second = -km.second; km.second = -km.second;
keyMatrixMap.insert(km); keyMatrixMap.insert(km);
@ -244,7 +248,7 @@ QP RawQP::makeQP() {
} }
for (auto kv : IL) { for (auto kv : IL) {
std::map<Key, Matrix11> keyMatrixMap; std::map < Key, Matrix11 > keyMatrixMap;
for (auto km : kv.second) { for (auto km : kv.second) {
keyMatrixMap.insert(km); keyMatrixMap.insert(km);
} }
@ -260,7 +264,7 @@ QP RawQP::makeQP() {
LinearInequality(k, I_1x1, up[k], dual_key_num++)); LinearInequality(k, I_1x1, up[k], dual_key_num++));
if (lo.count(k) == 1) if (lo.count(k) == 1)
madeQP.inequalities.push_back( madeQP.inequalities.push_back(
LinearInequality(k, -I_1x1, lo[k], dual_key_num++)); LinearInequality(k, -I_1x1, -lo[k], dual_key_num++));
else else
madeQP.inequalities.push_back( madeQP.inequalities.push_back(
LinearInequality(k, -I_1x1, 0, dual_key_num++)); LinearInequality(k, -I_1x1, 0, dual_key_num++));