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

Merged in feature/pcg_performance (pull request #413) 

Feature/pcg performance
release/4.3a0
Frank Dellaert 2019-04-14 03:14:23 +00:00
parent c8980b9b2b 948367603b
commit d0a73b42fb
18 changed files with 149 additions and 148 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -1,4 +1,5 @@
/build*
/debug*
.idea
*.pyc
*.DS_Store

8
gtsam/linear/GaussianConditional.cpp
View File

@ -134,7 +134,7 @@ namespace gtsam {
VectorValues result;
DenseIndex vectorPosition = 0;
for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
result.insert(*frontal, solution.segment(vectorPosition, getDim(frontal)));
result.emplace(*frontal, solution.segment(vectorPosition, getDim(frontal)));
vectorPosition += getDim(frontal);
}
@ -162,7 +162,7 @@ namespace gtsam {
VectorValues result;
DenseIndex vectorPosition = 0;
for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal)));
result.emplace(*frontal, soln.segment(vectorPosition, getDim(frontal)));
vectorPosition += getDim(frontal);
}
@ -172,13 +172,13 @@ namespace gtsam {
/* ************************************************************************* */
void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals()));
frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R()));
frontalVec = get_R().transpose().triangularView<Eigen::Lower>().solve(frontalVec);
// Check for indeterminant solution
if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front());
for (const_iterator it = beginParents(); it!= endParents(); it++)
gy[*it] += -1.0 * Matrix(getA(it)).transpose() * frontalVec;
gy[*it].noalias() += -1.0 * getA(it).transpose() * frontalVec;
// Scale by sigmas
if (model_)

4
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -61,7 +61,7 @@ namespace gtsam {
for (GaussianFactor::const_iterator it = gf->begin(); it != gf->end(); it++) {
map<Key,size_t>::iterator it2 = spec.find(*it);
if ( it2 == spec.end() ) {
spec.insert(make_pair(*it, gf->getDim(it)));
spec.emplace(*it, gf->getDim(it));
}
}
}
@ -246,7 +246,7 @@ namespace gtsam {
if (blocks.count(j))
blocks[j] += Bj;
else
blocks.insert(make_pair(j,Bj));
blocks.emplace(j,Bj);
}
}
return blocks;

6
gtsam/linear/HessianFactor.cpp
View File

@ -305,7 +305,7 @@ Matrix HessianFactor::information() const {
VectorValues HessianFactor::hessianDiagonal() const {
VectorValues d;
for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
d.insert(keys_[j], info_.diagonal(j));
d.emplace(keys_[j], info_.diagonal(j));
}
return d;
}
@ -436,7 +436,7 @@ VectorValues HessianFactor::gradientAtZero() const {
VectorValues g;
size_t n = size();
for (size_t j = 0; j < n; ++j)
g.insert(keys_[j], -info_.aboveDiagonalBlock(j, n));
g.emplace(keys_[j], -info_.aboveDiagonalBlock(j, n));
return g;
}
@ -513,7 +513,7 @@ VectorValues HessianFactor::solve() {
std::size_t offset = 0;
for (DenseIndex j = 0; j < (DenseIndex)n; ++j) {
const DenseIndex dim = info_.getDim(j);
delta.insert(keys_[j], solution.segment(offset, dim));
delta.emplace(keys_[j], solution.segment(offset, dim));
offset += dim;
}

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;
}

23
gtsam/linear/IterativeSolver.h
View File

@ -32,8 +32,8 @@
namespace gtsam {
// Forward declarations
struct KeyInfoEntry;
class KeyInfo;
class KeyInfoEntry;
class GaussianFactorGraph;
class Values;
class VectorValues;
@ -109,27 +109,14 @@ public:
/**
* Handy data structure for iterative solvers
* key to (index, dimension, colstart)
* key to (index, dimension, start)
*/
class GTSAM_EXPORT KeyInfoEntry: public boost::tuple<Key, size_t, Key> {
public:
typedef boost::tuple<Key, size_t, Key> Base;
struct GTSAM_EXPORT KeyInfoEntry {
size_t index, dim, start;
KeyInfoEntry() {
}
KeyInfoEntry(size_t idx, size_t d, Key start) :
Base(idx, d, start) {
}
size_t index() const {
return this->get<0>();
}
size_t dim() const {
return this->get<1>();
}
size_t colstart() const {
return this->get<2>();
index(idx), dim(d), start(start) {
}
};

48
gtsam/linear/JacobianFactor.cpp
View File

@ -430,10 +430,9 @@ Vector JacobianFactor::unweighted_error(const VectorValues& c) const {
/* ************************************************************************* */
Vector JacobianFactor::error_vector(const VectorValues& c) const {
if (model_)
return model_->whiten(unweighted_error(c));
else
return unweighted_error(c);
Vector e = unweighted_error(c);
if (model_) model_->whitenInPlace(e);
return e;
}
/* ************************************************************************* */
@ -474,10 +473,10 @@ VectorValues JacobianFactor::hessianDiagonal() const {
for (size_t k = 0; k < nj; ++k) {
Vector column_k = Ab_(pos).col(k);
if (model_)
column_k = model_->whiten(column_k);
model_->whitenInPlace(column_k);
dj(k) = dot(column_k, column_k);
}
d.insert(j, dj);
d.emplace(j, dj);
}
return d;
}
@ -496,7 +495,7 @@ map<Key, Matrix> JacobianFactor::hessianBlockDiagonal() const {
Matrix Aj = Ab_(pos);
if (model_)
Aj = model_->Whiten(Aj);
blocks.insert(make_pair(j, Aj.transpose() * Aj));
blocks.emplace(j, Aj.transpose() * Aj);
}
return blocks;
}
@ -541,29 +540,38 @@ void JacobianFactor::updateHessian(const KeyVector& infoKeys,
/* ************************************************************************* */
Vector JacobianFactor::operator*(const VectorValues& x) const {
Vector Ax = Vector::Zero(Ab_.rows());
Vector Ax(Ab_.rows());
Ax.setZero();
if (empty())
return Ax;
// Just iterate over all A matrices and multiply in correct config part
for (size_t pos = 0; pos < size(); ++pos)
Ax += Ab_(pos) * x[keys_[pos]];
for (size_t pos = 0; pos < size(); ++pos) {
// http://eigen.tuxfamily.org/dox/TopicWritingEfficientProductExpression.html
Ax.noalias() += Ab_(pos) * x[keys_[pos]];
}
return model_ ? model_->whiten(Ax) : Ax;
if (model_) model_->whitenInPlace(Ax);
return Ax;
}
/* ************************************************************************* */
void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e,
VectorValues& x) const {
Vector E = alpha * (model_ ? model_->whiten(e) : e);
Vector E(e.size());
E.noalias() = alpha * e;
if (model_) model_->whitenInPlace(E);
// Just iterate over all A matrices and insert Ai^e into VectorValues
for (size_t pos = 0; pos < size(); ++pos) {
Key j = keys_[pos];
// Create the value as a zero vector if it does not exist.
pair<VectorValues::iterator, bool> xi = x.tryInsert(j, Vector());
if (xi.second)
xi.first->second = Vector::Zero(getDim(begin() + pos));
xi.first->second += Ab_(pos).transpose()*E;
const Key j = keys_[pos];
// To avoid another malloc if key exists, we explicitly check
auto it = x.find(j);
if (it != x.end()) {
// http://eigen.tuxfamily.org/dox/TopicWritingEfficientProductExpression.html
it->second.noalias() += Ab_(pos).transpose() * E;
} else {
x.emplace(j, Ab_(pos).transpose() * E);
}
}
}
@ -625,8 +633,8 @@ VectorValues JacobianFactor::gradientAtZero() const {
Vector b = getb();
// Gradient is really -A'*b / sigma^2
// transposeMultiplyAdd will divide by sigma once, so we need one more
Vector b_sigma = model_ ? model_->whiten(b) : b;
this->transposeMultiplyAdd(-1.0, b_sigma, g); // g -= A'*b/sigma^2
if (model_) model_->whitenInPlace(b);
this->transposeMultiplyAdd(-1.0, b, g); // g -= A'*b/sigma^2
return g;
}

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;
}

85
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;
}
}
@ -658,21 +657,22 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo,
const KeyVector &keys) {
/* a cache of starting index and dim */
vector<std::pair<size_t, size_t> > cache;
cache.reserve(3);
/* figure out dimension by traversing the keys */
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;
@ -680,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;
}
}
@ -695,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;
}

27
gtsam/linear/VectorValues.cpp
View File

@ -51,7 +51,7 @@ namespace gtsam {
Key key;
size_t n;
boost::tie(key, n) = v;
values_.insert(make_pair(key, x.segment(j, n)));
values_.emplace(key, x.segment(j, n));
j += n;
}
}
@ -60,7 +60,7 @@ namespace gtsam {
VectorValues::VectorValues(const Vector& x, const Scatter& scatter) {
size_t j = 0;
for (const SlotEntry& v : scatter) {
values_.insert(make_pair(v.key, x.segment(j, v.dimension)));
values_.emplace(v.key, x.segment(j, v.dimension));
j += v.dimension;
}
}
@ -70,14 +70,12 @@ namespace gtsam {
{
VectorValues result;
for(const KeyValuePair& v: other)
result.values_.insert(make_pair(v.first, Vector::Zero(v.second.size())));
result.values_.emplace(v.first, Vector::Zero(v.second.size()));
return result;
}
/* ************************************************************************* */
VectorValues::iterator VectorValues::insert(const std::pair<Key, Vector>& key_value) {
// Note that here we accept a pair with a reference to the Vector, but the Vector is copied as
// it is inserted into the values_ map.
std::pair<iterator, bool> result = values_.insert(key_value);
if(!result.second)
throw std::invalid_argument(
@ -86,6 +84,16 @@ namespace gtsam {
return result.first;
}
/* ************************************************************************* */
VectorValues::iterator VectorValues::emplace(Key j, const Vector& value) {
std::pair<iterator, bool> result = values_.emplace(j, value);
if(!result.second)
throw std::invalid_argument(
"Requested to emplace variable '" + DefaultKeyFormatter(j)
+ "' already in this VectorValues.");
return result.first;
}
/* ************************************************************************* */
void VectorValues::update(const VectorValues& values)
{
@ -132,8 +140,7 @@ namespace gtsam {
bool VectorValues::equals(const VectorValues& x, double tol) const {
if(this->size() != x.size())
return false;
typedef boost::tuple<value_type, value_type> ValuePair;
for(const ValuePair& values: boost::combine(*this, x)) {
for(const auto& values: boost::combine(*this, x)) {
if(values.get<0>().first != values.get<1>().first ||
!equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol))
return false;
@ -240,7 +247,7 @@ namespace gtsam {
VectorValues result;
// The result.end() hint here should result in constant-time inserts
for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2)
result.values_.insert(result.end(), make_pair(j1->first, j1->second + j2->second));
result.values_.emplace(j1->first, j1->second + j2->second);
return result;
}
@ -298,7 +305,7 @@ namespace gtsam {
VectorValues result;
// The result.end() hint here should result in constant-time inserts
for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2)
result.values_.insert(result.end(), make_pair(j1->first, j1->second - j2->second));
result.values_.emplace(j1->first, j1->second - j2->second);
return result;
}
@ -314,7 +321,7 @@ namespace gtsam {
{
VectorValues result;
for(const VectorValues::KeyValuePair& key_v: v)
result.values_.insert(result.values_.end(), make_pair(key_v.first, a * key_v.second));
result.values_.emplace(key_v.first, a * key_v.second);
return result;
}

36
gtsam/linear/VectorValues.h
View File

@ -50,9 +50,9 @@ namespace gtsam {
* Example:
* \code
VectorValues values;
values.insert(3, Vector3(1.0, 2.0, 3.0));
values.insert(4, Vector2(4.0, 5.0));
values.insert(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0).finished());
values.emplace(3, Vector3(1.0, 2.0, 3.0));
values.emplace(4, Vector2(4.0, 5.0));
values.emplace(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0).finished());
// Prints [ 3.0 4.0 ]
gtsam::print(values[1]);
@ -64,18 +64,7 @@ namespace gtsam {
*
* <h2>Advanced Interface and Performance Information</h2>
*
* For advanced usage, or where speed is important:
* - Allocate space ahead of time using a pre-allocating constructor
* (\ref AdvancedConstructors "Advanced Constructors"), Zero(),
* SameStructure(), resize(), or append(). Do not use
* insert(Key, const Vector&), which always has to re-allocate the
* internal vector.
* - The vector() function permits access to the underlying Vector, for
* doing mathematical or other operations that require all values.
* - operator[]() returns a SubVector view of the underlying Vector,
* without copying any data.
*
* Access is through the variable index j, and returns a SubVector,
* Access is through the variable Key j, and returns a SubVector,
* which is a view on the underlying data structure.
*
* This class is additionally used in gradient descent and dog leg to store the gradient.
@ -183,15 +172,21 @@ namespace gtsam {
* j is already used.
* @param value The vector to be inserted.
* @param j The index with which the value will be associated. */
iterator insert(Key j, const Vector& value) {
return insert(std::make_pair(j, value));
}
iterator insert(const std::pair<Key, Vector>& key_value);
/** Emplace a vector \c value with key \c j. Throws an invalid_argument exception if the key \c
* j is already used.
* @param value The vector to be inserted.
* @param j The index with which the value will be associated. */
iterator emplace(Key j, const Vector& value);
/** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c
* j is already used.
* @param value The vector to be inserted.
* @param j The index with which the value will be associated. */
iterator insert(const std::pair<Key, Vector>& key_value);
iterator insert(Key j, const Vector& value) {
return insert(std::make_pair(j, value));
}
/** Insert all values from \c values. Throws an invalid_argument exception if any keys to be
* inserted are already used. */
@ -202,7 +197,8 @@ namespace gtsam {
* exist, it is inserted and an iterator pointing to the new element, along with 'true', is
* returned. */
std::pair<iterator, bool> tryInsert(Key j, const Vector& value) {
return values_.insert(std::make_pair(j, value)); }
return values_.emplace(j, value);
}
/** Erase the vector with the given key, or throw std::out_of_range if it does not exist */
void erase(Key var) {

6
gtsam/linear/linearAlgorithms-inst.h
View File

@ -56,7 +56,7 @@ namespace gtsam
myData.parentData = parentData;
// Take any ancestor results we'll need
for(Key parent: clique->conditional_->parents())
myData.cliqueResults.insert(std::make_pair(parent, myData.parentData->cliqueResults.at(parent)));
myData.cliqueResults.emplace(parent, myData.parentData->cliqueResults.at(parent));
// Solve and store in our results
{
@ -99,8 +99,8 @@ namespace gtsam
DenseIndex vectorPosition = 0;
for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) {
VectorValues::const_iterator r =
collectedResult.insert(*frontal, solution.segment(vectorPosition, c.getDim(frontal)));
myData.cliqueResults.insert(make_pair(r->first, r));
collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal)));
myData.cliqueResults.emplace(r->first, r);
vectorPosition += c.getDim(frontal);
}
}

21
gtsam/linear/tests/testJacobianFactor.cpp
View File

@ -322,27 +322,30 @@ TEST(JacobianFactor, matrices)
/* ************************************************************************* */
TEST(JacobianFactor, operators )
{
SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
const double sigma = 0.1;
SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2, sigma);
Matrix I = I_2x2;
Vector b = Vector2(0.2,-0.1);
JacobianFactor lf(1, -I, 2, I, b, sigma0_1);
VectorValues c;
c.insert(1, Vector2(10.,20.));
c.insert(2, Vector2(30.,60.));
VectorValues x;
Vector2 x1(10,20), x2(30,60);
x.insert(1, x1);
x.insert(2, x2);
// test A*x
Vector expectedE = Vector2(200.,400.);
Vector actualE = lf * c;
Vector expectedE = (x2 - x1)/sigma;
Vector actualE = lf * x;
EXPECT(assert_equal(expectedE, actualE));
// test A^e
VectorValues expectedX;
expectedX.insert(1, Vector2(-2000.,-4000.));
expectedX.insert(2, Vector2(2000., 4000.));
const double alpha = 0.5;
expectedX.insert(1, - alpha * expectedE /sigma);
expectedX.insert(2, alpha * expectedE /sigma);
VectorValues actualX = VectorValues::Zero(expectedX);
lf.transposeMultiplyAdd(1.0, actualE, actualX);
lf.transposeMultiplyAdd(alpha, actualE, actualX);
EXPECT(assert_equal(expectedX, actualX));
// test gradient at zero

6
gtsam/slam/InitializePose3.cpp
View File

@ -82,7 +82,11 @@ Values InitializePose3::normalizeRelaxedRotations(
for(const auto& it: relaxedRot3) {
Key key = it.first;
if (key != kAnchorKey) {
Matrix3 R; R << it.second;
Matrix3 R;
R << Eigen::Map<const Matrix3>(it.second.data()); // Recover M from vectorized
// ClosestTo finds rotation matrix closest to H in Frobenius sense
// Rot3 initRot = Rot3::ClosestTo(M.transpose());
Matrix U, V; Vector s;
svd(R.transpose(), U, s, V);

1
gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
View File

@ -46,7 +46,6 @@ static double rankTol = 1.0;
template<class CALIBRATION>
PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
const PinholeCamera<CALIBRATION>& camera) {
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
Pose3 cameraPose = camera.pose();

2
gtsam_unstable/examples/SmartProjectionFactorExample.cpp
View File

@ -43,8 +43,6 @@ using namespace std;
using namespace gtsam;
int main(int argc, char** argv){
typedef PinholePose<Cal3_S2> Camera;
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
Values initial_estimate;

1
gtsam_unstable/partition/GenericGraph.cpp
View File

@ -353,7 +353,6 @@ namespace gtsam { namespace partition {
void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
const std::vector<int>& dictionary, GenericGraph3D& reducedGraph) {
typedef size_t CameraKey;
typedef size_t LandmarkKey;
// get a mapping from each landmark to its connected cameras
vector<vector<LandmarkKey> > cameraToLandmarks(dictionary.size());

1
tests/testExpressionFactor.cpp
View File

@ -243,7 +243,6 @@ TEST(ExpressionFactor, Shallow) {
// traceExecution of shallow tree
typedef internal::UnaryExpression<Point2, Point3> Unary;
typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary;
size_t size = expression.traceSize();
internal::ExecutionTraceStorage traceStorage[size];
internal::ExecutionTrace<Point2> trace;