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