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

Fixed up iterative methods to use struct

release/4.3a0
Frank Dellaert 2019-04-08 16:53:27 -04:00
parent d814710575
commit 2cedda703c
3 changed files with 51 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/linear/IterativeSolver.cpp
View File

@ -117,7 +117,7 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg) {
for (size_t i = 0; i < n; ++i) {
const size_t key = ordering_[i];
const size_t dim = colspec.find(key)->second;
insert(make_pair(key, KeyInfoEntry(i, dim, start)));
this->emplace(key, KeyInfoEntry(i, dim, start));
start += dim;
}
numCols_ = start;
@ -126,8 +126,8 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg) {
/****************************************************************************/
vector<size_t> KeyInfo::colSpec() const {
std::vector<size_t> result(size(), 0);
for ( const KeyInfo::value_type &item: *this ) {
result[item.second.index()] = item.second.dim();
for ( const auto &item: *this ) {
result[item.second.index] = item.second.dim;
}
return result;
}
@ -135,8 +135,8 @@ vector<size_t> KeyInfo::colSpec() const {
/****************************************************************************/
VectorValues KeyInfo::x0() const {
VectorValues result;
for ( const KeyInfo::value_type &item: *this ) {
result.insert(item.first, Vector::Zero(item.second.dim()));
for ( const auto &item: *this ) {
result.emplace(item.first, Vector::Zero(item.second.dim));
}
return result;
}

9
gtsam/linear/PCGSolver.cpp
View File

@ -89,7 +89,7 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const {
VectorValues vvX = buildVectorValues(x, keyInfo_);
// VectorValues form of A'Ax for multiplyHessianAdd
VectorValues vvAtAx;
VectorValues vvAtAx = keyInfo_.x0(); // crucial for performance
// vvAtAx += 1.0 * A'Ax for each factor
gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx);
@ -132,14 +132,14 @@ VectorValues buildVectorValues(const Vector &v, const Ordering &ordering,
DenseIndex offset = 0;
for (size_t i = 0; i < ordering.size(); ++i) {
const Key &key = ordering[i];
const Key key = ordering[i];
map<Key, size_t>::const_iterator it = dimensions.find(key);
if (it == dimensions.end()) {
throw invalid_argument(
"buildVectorValues: inconsistent ordering and dimensions");
}
const size_t dim = it->second;
result.insert(key, v.segment(offset, dim));
result.emplace(key, v.segment(offset, dim));
offset += dim;
}
@ -150,8 +150,7 @@ VectorValues buildVectorValues(const Vector &v, const Ordering &ordering,
VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) {
VectorValues result;
for ( const KeyInfo::value_type &item: keyInfo ) {
result.insert(item.first,
v.segment(item.second.colstart(), item.second.dim()));
result.emplace(item.first, v.segment(item.second.start, item.second.dim));
}
return result;
}

84
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -99,8 +99,8 @@ static vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {
const double value = rand() / denominator;
/* binary search the interval containing "value" */
const auto it = std::lower_bound(acc.begin(), acc.end(), value);
const size_t idx = it - acc.begin();
result.push_back(idx);
const size_t index = it - acc.begin();
result.push_back(index);
}
return result;
}
@ -129,10 +129,10 @@ static vector<size_t> UniqueSampler(const vector<double> &weight, const size_t n
/* sampling and cache results */
vector<size_t> samples = iidSampler(localWeights, n-count);
for ( const size_t &id: samples ) {
if ( touched[id] == false ) {
touched[id] = true ;
result.push_back(id);
for ( const size_t &index: samples ) {
if ( touched[index] == false ) {
touched[index] = true ;
result.push_back(index);
if ( ++count >= n ) break;
}
}
@ -143,8 +143,8 @@ static vector<size_t> UniqueSampler(const vector<double> &weight, const size_t n
/****************************************************************************/
Subgraph::Subgraph(const vector<size_t> &indices) {
edges_.reserve(indices.size());
for ( const size_t &idx: indices ) {
edges_.emplace_back(idx, 1.0);
for ( const size_t &index: indices ) {
edges_.emplace_back(index, 1.0);
}
}
@ -296,12 +296,12 @@ vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg,
/****************************************************************/
vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
vector<size_t> result ;
size_t idx = 0;
size_t index = 0;
for (const auto &factor : gfg) {
if ( factor->size() == 1 ) {
result.push_back(idx);
result.push_back(index);
}
idx++;
index++;
}
return result;
}
@ -309,14 +309,14 @@ vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
/****************************************************************/
vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
vector<size_t> result ;
size_t idx = 0;
size_t index = 0;
for ( const GaussianFactor::shared_ptr &gf: gfg ) {
if ( gf->size() == 2 ) {
const Key k0 = gf->keys()[0], k1 = gf->keys()[1];
if ( (k1-k0) == 1 || (k0-k1) == 1 )
result.push_back(idx);
result.push_back(index);
}
idx++;
index++;
}
return result;
}
@ -343,13 +343,13 @@ vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
/* traversal */
while ( !q.empty() ) {
const size_t head = q.front(); q.pop();
for ( const size_t id: variableIndex[head] ) {
const GaussianFactor &gf = *gfg[id];
for ( const size_t index: variableIndex[head] ) {
const GaussianFactor &gf = *gfg[index];
for ( const size_t key: gf.keys() ) {
if ( flags.count(key) == 0 ) {
q.push(key);
flags.insert(key);
result.push_back(id);
result.push_back(index);
}
}
}
@ -363,7 +363,7 @@ vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
const vector<double> &weights) const {
const VariableIndex variableIndex(gfg);
const size_t n = variableIndex.size();
const vector<size_t> idx = sort_idx(weights) ;
const vector<size_t> sortedIndices = sort_idx(weights) ;
/* initialize buffer */
vector<size_t> result;
@ -373,16 +373,16 @@ vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
DSFVector dsf(n);
size_t count = 0 ; double sum = 0.0 ;
for (const size_t id: idx) {
const GaussianFactor &gf = *gfg[id];
for (const size_t index: sortedIndices) {
const GaussianFactor &gf = *gfg[index];
const auto keys = gf.keys();
if ( keys.size() != 2 ) continue;
const size_t u = ordering.find(keys[0])->second,
v = ordering.find(keys[1])->second;
if ( dsf.find(u) != dsf.find(v) ) {
dsf.merge(u, v) ;
result.push_back(id) ;
sum += weights[id] ;
result.push_back(index) ;
sum += weights[index] ;
if ( ++count == n-1 ) break ;
}
}
@ -418,8 +418,8 @@ Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg
}
// Downweight the tree edges to zero.
for ( const size_t id: tree ) {
weights[id] = 0.0;
for ( const size_t index: tree ) {
weights[index] = 0.0;
}
/* decide how many edges to augment */
@ -614,8 +614,8 @@ void SubgraphPreconditioner::transposeSolve(const Vector &y, Vector &x) const {
/* in place back substitute */
for (const auto &cg : *Rc1_) {
const Vector rhsFrontal = getSubvector(
x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals());
const Vector rhsFrontal = getSubvector(x, keyInfo_, frontalKeys);
const Vector solFrontal =
cg->get_R().transpose().triangularView<Eigen::Lower>().solve(
rhsFrontal);
@ -625,13 +625,12 @@ void SubgraphPreconditioner::transposeSolve(const Vector &y, Vector &x) const {
throw IndeterminantLinearSystemException(cg->keys().front());
/* assign subvector of sol to the frontal variables */
setSubvector(solFrontal, keyInfo_,
KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
setSubvector(solFrontal, keyInfo_, frontalKeys, x);
/* substract from parent variables */
for (auto it = cg->beginParents(); it != cg->endParents(); it++) {
const KeyInfoEntry &info = keyInfo_.find(*it)->second;
Eigen::Map<Vector> rhsParent(x.data() + info.colstart(), info.dim(), 1);
const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
Eigen::Map<Vector> rhsParent(x.data() + entry.start, entry.dim, 1);
rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal;
}
}
@ -664,16 +663,16 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo,
size_t dim = 0;
for (const Key &key : keys) {
const KeyInfoEntry &entry = keyInfo.find(key)->second;
cache.emplace_back(entry.colstart(), entry.dim());
dim += entry.dim();
cache.emplace_back(entry.start, entry.dim);
dim += entry.dim;
}
/* use the cache to fill the result */
Vector result = Vector::Zero(dim);
size_t idx = 0;
Vector result(dim);
size_t index = 0;
for (const auto &p : cache) {
result.segment(idx, p.second) = src.segment(p.first, p.second);
idx += p.second;
result.segment(index, p.second) = src.segment(p.first, p.second);
index += p.second;
}
return result;
@ -681,11 +680,12 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo,
/*****************************************************************************/
void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst) {
size_t idx = 0;
size_t index = 0;
for ( const Key &key: keys ) {
const KeyInfoEntry &entry = keyInfo.find(key)->second;
dst.segment(entry.colstart(), entry.dim()) = src.segment(idx, entry.dim()) ;
idx += entry.dim();
const size_t keyDim = entry.dim;
dst.segment(entry.start, keyDim) = src.segment(index, keyDim) ;
index += keyDim;
}
}
@ -696,9 +696,9 @@ GaussianFactorGraph::shared_ptr buildFactorSubgraph(
auto result = boost::make_shared<GaussianFactorGraph>();
result->reserve(subgraph.size());
for ( const SubgraphEdge &e: subgraph ) {
const size_t idx = e.index();
if ( clone ) result->push_back(gfg[idx]->clone());
else result->push_back(gfg[idx]);
const size_t index = e.index();
if ( clone ) result->push_back(gfg[index]->clone());
else result->push_back(gfg[index]);
}
return result;
}