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

Merge branch 'refs/heads/isam2-chain-optimization' into trunk

release/4.3a0
Richard Roberts 2013-02-05 21:52:44 +00:00
parent 6572400bc3 f2a371e6c1
commit 5b96d1abf9
40 changed files with 726 additions and 678 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam.h
View File

@ -724,7 +724,6 @@ class Permutation {
// FIXME: Cannot currently wrap std::vector
//static gtsam::Permutation PullToFront(const vector<size_t>& toFront, size_t size, bool filterDuplicates);
//static gtsam::Permutation PushToBack(const vector<size_t>& toBack, size_t size, bool filterDuplicates = false);
gtsam::Permutation* partialPermutation(const gtsam::Permutation& selector, const gtsam::Permutation& partialPermutation) const;
};
class IndexFactor {
@ -772,7 +771,6 @@ class IndexConditional {
gtsam::IndexFactor* toFactor() const;
//Advanced interface
bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
void permuteWithInverse(const gtsam::Permutation& inversePermutation);
};
@ -795,7 +793,6 @@ virtual class BayesNet {
void push_front(This& conditional);
void pop_front();
void permuteWithInverse(const gtsam::Permutation& inversePermutation);
bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
};
#include <gtsam/inference/BayesTree.h>
@ -840,7 +837,6 @@ virtual class BayesTreeClique {
// derived_ptr parent() const { return parent_.lock(); }
void permuteWithInverse(const gtsam::Permutation& inversePermutation);
bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
// FIXME: need wrapped versions graphs, BayesNet
// BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
@ -868,7 +864,6 @@ virtual class SymbolicBayesNet : gtsam::SymbolicBayesNetBase {
//Advanced Interface
void pop_front();
void permuteWithInverse(const gtsam::Permutation& inversePermutation);
bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
};
typedef gtsam::BayesTreeClique<gtsam::IndexConditional> SymbolicBayesTreeClique;
@ -1050,12 +1045,11 @@ class VectorValues {
//Standard Interface
size_t size() const;
size_t dim(size_t j) const;
size_t dim() const;
bool exists(size_t j) const;
void print(string s) const;
bool equals(const gtsam::VectorValues& expected, double tol) const;
void insert(size_t j, Vector value);
Vector vector() const;
Vector asVector() const;
Vector at(size_t j) const;
//Advanced Interface
@ -1821,7 +1815,6 @@ virtual class ISAM2Clique {
void print(string s);
void permuteWithInverse(const gtsam::Permutation& inversePermutation);
bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
};
class ISAM2Result {

12
gtsam/inference/BayesNet-inl.h
View File

@ -117,18 +117,6 @@ namespace gtsam {
}
}
/* ************************************************************************* */
template<class CONDITIONAL>
bool BayesNet<CONDITIONAL>::permuteSeparatorWithInverse(
const Permutation& inversePermutation) {
bool separatorChanged = false;
BOOST_FOREACH(sharedConditional conditional, conditionals_) {
if (conditional->permuteSeparatorWithInverse(inversePermutation))
separatorChanged = true;
}
return separatorChanged;
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::push_back(const BayesNet<CONDITIONAL>& bn) {

7
gtsam/inference/BayesNet.h
View File

@ -220,13 +220,6 @@ public:
/** Permute the variables in the BayesNet */
void permuteWithInverse(const Permutation& inversePermutation);
/**
* Permute the variables when only separator variables need to be permuted.
* Returns true if any reordered variables appeared in the separator and
* false if not.
*/
bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
iterator begin() {return conditionals_.begin();} ///<TODO: comment
iterator end() {return conditionals_.end();} ///<TODO: comment
reverse_iterator rbegin() {return conditionals_.rbegin();} ///<TODO: comment

2
gtsam/inference/BayesTree.h
View File

@ -92,7 +92,7 @@ namespace gtsam {
};
/** Map from indices to Clique */
typedef std::deque<sharedClique> Nodes;
typedef std::vector<sharedClique> Nodes;
protected:

17
gtsam/inference/BayesTreeCliqueBase-inl.h
View File

@ -134,22 +134,19 @@ namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
bool BayesTreeCliqueBase<DERIVED, CONDITIONAL>::permuteSeparatorWithInverse(
const Permutation& inversePermutation) {
bool changed = conditional_->permuteSeparatorWithInverse(
inversePermutation);
bool BayesTreeCliqueBase<DERIVED, CONDITIONAL>::reduceSeparatorWithInverse(
const internal::Reduction& inverseReduction)
{
bool changed = conditional_->reduceSeparatorWithInverse(inverseReduction);
#ifndef NDEBUG
if(!changed) {
BOOST_FOREACH(Index& separatorKey, conditional_->parents()) {assert(separatorKey == inversePermutation[separatorKey]);}
BOOST_FOREACH(const derived_ptr& child, children_) {
assert(child->permuteSeparatorWithInverse(inversePermutation) == false);
}
assert(child->reduceSeparatorWithInverse(inverseReduction) == false); }
}
#endif
if (changed) {
if(changed) {
BOOST_FOREACH(const derived_ptr& child, children_) {
(void) child->permuteSeparatorWithInverse(inversePermutation);
}
(void) child->reduceSeparatorWithInverse(inverseReduction); }
}
assertInvariants();
return changed;

2
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -183,7 +183,7 @@ namespace gtsam {
* traversing the whole tree. Returns whether any separator variables in
* this subtree were reordered.
*/
bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction);
/** return the conditional P(S|Root) on the separator given the root */
BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;

14
gtsam/inference/IndexConditional.cpp
View File

@ -44,16 +44,16 @@ namespace gtsam {
}
/* ************************************************************************* */
bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
#ifndef NDEBUG
BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); }
#endif
bool IndexConditional::reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) {
#ifndef NDEBUG
BOOST_FOREACH(KeyType key, frontals()) { assert(inverseReduction.find(key) == inverseReduction.end()); }
#endif
bool parentChanged = false;
BOOST_FOREACH(KeyType& parent, parents()) {
KeyType newParent = inversePermutation[parent];
if(parent != newParent) {
internal::Reduction::const_iterator it = inverseReduction.find(parent);
if(it != inverseReduction.end()) {
parentChanged = true;
parent = newParent;
parent = it->second;
}
}
assertInvariants();

8
gtsam/inference/IndexConditional.h
View File

@ -112,7 +112,13 @@ namespace gtsam {
* Returns true if any reordered variables appeared in the separator and
* false if not.
*/
bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
//bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
/** Permute the variables when only separator variables need to be permuted.
* Returns true if any reordered variables appeared in the separator and
* false if not.
*/
bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction);
/**
* Permutes the Conditional, but for efficiency requires the permutation

33
gtsam/inference/Permutation.cpp
View File

@ -123,18 +123,6 @@ Permutation::shared_ptr Permutation::permute(const Permutation& permutation) con
return result;
}
/* ************************************************************************* */
Permutation::shared_ptr Permutation::partialPermutation(
const Permutation& selector, const Permutation& partialPermutation) const {
assert(selector.size() == partialPermutation.size());
Permutation::shared_ptr result(new Permutation(*this));
for(Index subsetPos=0; subsetPos<selector.size(); ++subsetPos)
(*result)[selector[subsetPos]] = (*this)[selector[partialPermutation[subsetPos]]];
return result;
}
/* ************************************************************************* */
Permutation::shared_ptr Permutation::inverse() const {
Permutation::shared_ptr result(new Permutation(this->size()));
@ -161,6 +149,16 @@ namespace internal {
return result;
}
/* ************************************************************************* */
Reduction Reduction::CreateFromPartialPermutation(const Permutation& selector, const Permutation& p) {
if(selector.size() != p.size())
throw invalid_argument("internal::Reduction::CreateFromPartialPermutation called with selector and permutation of different sizes");
Reduction result;
for(size_t dstSlot = 0; dstSlot < p.size(); ++dstSlot)
result.insert(make_pair(selector[dstSlot], selector[p[dstSlot]]));
return result;
}
/* ************************************************************************* */
void Reduction::applyInverse(std::vector<Index>& js) const {
BOOST_FOREACH(Index& j, js) {
@ -183,10 +181,10 @@ namespace internal {
}
/* ************************************************************************* */
Index& Reduction::operator[](const Index& j) {
const Index& Reduction::operator[](const Index& j) {
iterator it = this->find(j);
if(it == this->end())
throw std::out_of_range("Index to Reduction::operator[] not present");
return j;
else
return it->second;
}
@ -195,7 +193,7 @@ namespace internal {
const Index& Reduction::operator[](const Index& j) const {
const_iterator it = this->find(j);
if(it == this->end())
throw std::out_of_range("Index to Reduction::operator[] not present");
return j;
else
return it->second;
}
@ -207,6 +205,11 @@ namespace internal {
cout << " " << p.first << " : " << p.second << endl;
}
/* ************************************************************************* */
bool Reduction::equals(const Reduction& other, double tol) const {
return (const Base&)(*this) == (const Base&)other;
}
/* ************************************************************************* */
Permutation createReducingPermutation(const std::set<Index>& indices) {
Permutation p(indices.size());

12
gtsam/inference/Permutation.h
View File

@ -167,14 +167,6 @@ public:
/// @name Advanced Interface
/// @{
/**
* A partial permutation, reorders the variables selected by selector through
* partialPermutation. selector and partialPermutation should have the same
* size, this is checked if NDEBUG is not defined.
*/
Permutation::shared_ptr partialPermutation(const Permutation& selector, const Permutation& partialPermutation) const;
iterator begin() { return rangeIndices_.begin(); } ///< Iterate through the indices
iterator end() { return rangeIndices_.end(); } ///< Iterate through the indices
@ -192,12 +184,14 @@ namespace internal {
typedef gtsam::FastMap<Index,Index> Base;
static Reduction CreateAsInverse(const Permutation& p);
static Reduction CreateFromPartialPermutation(const Permutation& selector, const Permutation& p);
void applyInverse(std::vector<Index>& js) const;
Permutation inverse() const;
Index& operator[](const Index& j);
const Index& operator[](const Index& j);
const Index& operator[](const Index& j) const;
void print(const std::string& s="") const;
bool equals(const Reduction& other, double tol = 1e-9) const;
};
// Reduce the variable indices so that those in the set are mapped to start at zero

16
gtsam/inference/VariableIndex.cpp
View File

@ -68,7 +68,7 @@ void VariableIndex::outputMetisFormat(ostream& os) const {
/* ************************************************************************* */
void VariableIndex::permuteInPlace(const Permutation& permutation) {
// Create new index and move references to data into it in permuted order
deque<VariableIndex::Factors> newIndex(this->size());
vector<VariableIndex::Factors> newIndex(this->size());
for(Index i = 0; i < newIndex.size(); ++i)
newIndex[i].swap(this->index_[permutation[i]]);
@ -76,6 +76,20 @@ void VariableIndex::permuteInPlace(const Permutation& permutation) {
index_.swap(newIndex);
}
/* ************************************************************************* */
void VariableIndex::permuteInPlace(const Permutation& selector, const Permutation& permutation) {
if(selector.size() != permutation.size())
throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes.");
// Create new index the size of the permuted entries
vector<VariableIndex::Factors> newIndex(selector.size());
// Permute the affected entries into the new index
for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot)
newIndex[dstSlot].swap(this->index_[selector[permutation[dstSlot]]]);
// Put the affected entries back in the new order
for(size_t slot = 0; slot < selector.size(); ++slot)
this->index_[selector[slot]].swap(newIndex[slot]);
}
/* ************************************************************************* */
void VariableIndex::removeUnusedAtEnd(size_t nToRemove) {
#ifndef NDEBUG

5
gtsam/inference/VariableIndex.h
View File

@ -48,7 +48,7 @@ public:
typedef Factors::const_iterator Factor_const_iterator;
protected:
std::deque<Factors> index_;
std::vector<Factors> index_;
size_t nFactors_; // Number of factors in the original factor graph.
size_t nEntries_; // Sum of involved variable counts of each factor.
@ -135,6 +135,9 @@ public:
/// Permute the variables in the VariableIndex according to the given permutation
void permuteInPlace(const Permutation& permutation);
/// Permute the variables in the VariableIndex according to the given partial permutation
void permuteInPlace(const Permutation& selector, const Permutation& permutation);
/** Remove unused empty variables at the end of the ordering (in debug mode
* verifies they are empty).
* @param nToRemove The number of unused variables at the end to remove

23
gtsam/inference/tests/testPermutation.cpp
View File

@ -25,6 +25,7 @@
using namespace gtsam;
using namespace std;
/* ************************************************************************* */
TEST(Permutation, Identity) {
Permutation expected(5);
expected[0] = 0;
@ -38,6 +39,7 @@ TEST(Permutation, Identity) {
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Permutation, PullToFront) {
Permutation expected(5);
expected[0] = 4;
@ -55,6 +57,7 @@ TEST(Permutation, PullToFront) {
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Permutation, PushToBack) {
Permutation expected(5);
expected[0] = 1;
@ -72,6 +75,7 @@ TEST(Permutation, PushToBack) {
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Permutation, compose) {
Permutation p1(5);
p1[0] = 1;
@ -104,6 +108,25 @@ TEST(Permutation, compose) {
LONGS_EQUAL(p1[p2[4]], actual[4]);
}
/* ************************************************************************* */
TEST(Reduction, CreateFromPartialPermutation) {
Permutation selector(3);
selector[0] = 2;
selector[1] = 4;
selector[2] = 6;
Permutation p(3);
p[0] = 2;
p[1] = 0;
p[2] = 1;
internal::Reduction expected;
expected.insert(make_pair(2,6));
expected.insert(make_pair(4,2));
expected.insert(make_pair(6,4));
internal::Reduction actual = internal::Reduction::CreateFromPartialPermutation(selector, p);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

6
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -607,11 +607,11 @@ break;
/* ************************************************************************* */
void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
r.vector() = Vector::Zero(r.dim());
r.setZero();
Index i = 0;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
SubVector &y = r[i];
Vector &y = r[i];
for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
y += Ai->getA(j) * x[*j];
}
@ -621,7 +621,7 @@ break;
/* ************************************************************************* */
void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x) {
x.vector() = Vector::Zero(x.dim());
x.setZero();
Index i = 0;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);

4
gtsam/linear/GaussianISAM.h
View File

@ -30,11 +30,11 @@ namespace gtsam {
class GaussianISAM : public ISAM<GaussianConditional> {
typedef ISAM<GaussianConditional> Super;
std::deque<size_t, boost::fast_pool_allocator<size_t> > dims_;
std::vector<size_t> dims_;
public:
typedef std::deque<size_t, boost::fast_pool_allocator<size_t> > Dims;
typedef std::vector<size_t> Dims;
/** Create an empty Bayes Tree */
GaussianISAM() : Super() {}

16
gtsam/linear/HessianFactor.cpp
View File

@ -325,17 +325,11 @@ double HessianFactor::error(const VectorValues& c) const {
// error 0.5*(f - 2*x'*g + x'*G*x)
const double f = constantTerm();
double xtg = 0, xGx = 0;
if (c.dim() == this->rows()) {
// If using the full vector values, this will reduce copying
xtg = c.vector().dot(linearTerm());
xGx = c.vector().transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>() * c.vector();
} else {
// extract the relevant subset of the VectorValues
// NOTE may not be as efficient
const Vector x = c.vector(this->keys());
xtg = x.dot(linearTerm());
xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>() * x;
}
// extract the relevant subset of the VectorValues
// NOTE may not be as efficient
const Vector x = c.vector(this->keys());
xtg = x.dot(linearTerm());
xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>() * x;
return 0.5 * (f - 2.0 * xtg + xGx);
}

241
gtsam/linear/VectorValues.cpp
View File

@ -20,29 +20,19 @@
#include <gtsam/inference/Permutation.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/iterator/counting_iterator.hpp>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
VectorValues::VectorValues(const VectorValues& other) {
*this = other;
}
/* ************************************************************************* */
VectorValues& VectorValues::operator=(const VectorValues& rhs) {
if(this != &rhs) {
resizeLike(rhs); // Copy structure
values_ = rhs.values_; // Copy values
}
return *this;
}
/* ************************************************************************* */
VectorValues VectorValues::Zero(const VectorValues& x) {
VectorValues cloned(SameStructure(x));
cloned.setZero();
return cloned;
VectorValues result;
result.values_.resize(x.size());
for(size_t j=0; j<x.size(); ++j)
result.values_[j] = Vector::Zero(x.values_[j].size());
return result;
}
/* ************************************************************************* */
@ -59,69 +49,44 @@ void VectorValues::insert(Index j, const Vector& value) {
if(exists(j))
throw invalid_argument("VectorValues: requested variable index to insert already exists.");
// Get vector of dimensions
FastVector<size_t> dimensions(size());
for(size_t k=0; k<maps_.size(); ++k)
dimensions[k] = maps_[k].rows();
// If this adds variables at the end, insert zero-length entries up to j
if(j >= size())
dimensions.insert(dimensions.end(), j+1-size(), 0);
values_.resize(j+1);
// Set correct dimension for j
dimensions[j] = value.rows();
// Make a copy to make assignment easier
VectorValues original(*this);
// Resize to accomodate new variable
resize(dimensions);
// Copy original variables
for(Index k = 0; k < original.size(); ++k)
if(k != j && exists(k))
operator[](k) = original[k];
// Copy new variable
operator[](j) = value;
// Assign value
values_[j] = value;
}
/* ************************************************************************* */
void VectorValues::print(const std::string& str, const IndexFormatter& formatter) const {
std::cout << str << ": " << size() << " elements\n";
for (Index var = 0; var < size(); ++var)
std::cout << " " << formatter(var) << ": \n" << operator[](var) << "\n";
std::cout << " " << formatter(var) << ": \n" << (*this)[var] << "\n";
std::cout.flush();
}
/* ************************************************************************* */
bool VectorValues::equals(const VectorValues& x, double tol) const {
return hasSameStructure(x) && equal_with_abs_tol(values_, x.values_, tol);
if(this->size() != x.size())
return false;
for(Index j=0; j < size(); ++j)
if(!equal_with_abs_tol(values_[j], x.values_[j], tol))
return false;
return true;
}
/* ************************************************************************* */
void VectorValues::resize(Index nVars, size_t varDim) {
maps_.clear();
maps_.reserve(nVars);
values_.resize(nVars * varDim);
int varStart = 0;
for (Index j = 0; j < nVars; ++j) {
maps_.push_back(values_.segment(varStart, varDim));
varStart += varDim;
}
values_.resize(nVars);
for(Index j = 0; j < nVars; ++j)
values_[j] = Vector(varDim);
}
/* ************************************************************************* */
void VectorValues::resizeLike(const VectorValues& other) {
values_.resize(other.dim());
// Create SubVectors referencing our values_ vector
maps_.clear();
maps_.reserve(other.size());
int varStart = 0;
BOOST_FOREACH(const SubVector& value, other) {
maps_.push_back(values_.segment(varStart, value.rows()));
varStart += value.rows();
}
values_.resize(other.size());
for(Index j = 0; j < other.size(); ++j)
values_[j].resize(other.values_[j].size());
}
/* ************************************************************************* */
@ -140,91 +105,133 @@ VectorValues VectorValues::Zero(Index nVars, size_t varDim) {
/* ************************************************************************* */
void VectorValues::setZero() {
values_.setZero();
BOOST_FOREACH(Vector& v, *this) {
v.setZero();
}
}
/* ************************************************************************* */
const Vector VectorValues::asVector() const {
return internal::extractVectorValuesSlices(*this,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(this->size()), true);
}
/* ************************************************************************* */
const Vector VectorValues::vector(const std::vector<Index>& indices) const {
return internal::extractVectorValuesSlices(*this, indices.begin(), indices.end());
}
/* ************************************************************************* */
bool VectorValues::hasSameStructure(const VectorValues& other) const {
if(this->size() != other.size())
return false;
for(size_t j=0; j<size(); ++j)
for(size_t j = 0; j < size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
if(this->maps_[j].rows() != other.maps_[j].rows())
if(this->values_[j].rows() != other.values_[j].rows())
return false;
return true;
}
/* ************************************************************************* */
VectorValues VectorValues::operator+(const VectorValues& c) const {
assert(this->hasSameStructure(c));
VectorValues result(SameStructure(c));
result.values_ = this->values_ + c.values_;
return result;
void VectorValues::permuteInPlace(const Permutation& permutation) {
// Allocate new values
Values newValues(this->size());
// Swap values from this VectorValues to the permuted VectorValues
for(size_t i = 0; i < this->size(); ++i)
newValues[i].swap(this->at(permutation[i]));
// Swap the values into this VectorValues
values_.swap(newValues);
}
/* ************************************************************************* */
VectorValues VectorValues::operator-(const VectorValues& c) const {
assert(this->hasSameStructure(c));
VectorValues result(SameStructure(c));
result.values_ = this->values_ - c.values_;
return result;
}
/* ************************************************************************* */
void VectorValues::operator+=(const VectorValues& c) {
assert(this->hasSameStructure(c));
this->values_ += c.values_;
}
/* ************************************************************************* */
VectorValues VectorValues::permute(const Permutation& permutation) const {
// Create result and allocate space
VectorValues lhs;
lhs.values_.resize(this->dim());
lhs.maps_.reserve(this->size());
// Copy values from this VectorValues to the permuted VectorValues
size_t lhsPos = 0;
for(size_t i = 0; i < this->size(); ++i) {
// Map the next LHS subvector to the next slice of the LHS vector
lhs.maps_.push_back(SubVector(lhs.values_, lhsPos, this->at(permutation[i]).size()));
// Copy the data from the RHS subvector to the LHS subvector
lhs.maps_[i] = this->at(permutation[i]);
// Increment lhs position
lhsPos += lhs.maps_[i].size();
}
return lhs;
void VectorValues::permuteInPlace(const Permutation& selector, const Permutation& permutation) {
if(selector.size() != permutation.size())
throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes.");
// Create new index the size of the permuted entries
Values reorderedEntries(selector.size());
// Permute the affected entries into the new index
for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot)
reorderedEntries[dstSlot].swap(values_[selector[permutation[dstSlot]]]);
// Put the affected entries back in the new order
for(size_t slot = 0; slot < selector.size(); ++slot)
values_[selector[slot]].swap(reorderedEntries[slot]);
}
/* ************************************************************************* */
void VectorValues::swap(VectorValues& other) {
this->values_.swap(other.values_);
this->maps_.swap(other.maps_);
}
/* ************************************************************************* */
Vector VectorValues::vector(const std::vector<Index>& indices) const {
if (indices.empty())
return Vector();
// find dimensions
size_t d = 0;
BOOST_FOREACH(const Index& idx, indices)
d += dim(idx);
// copy out values
Vector result(d);
size_t curHead = 0;
BOOST_FOREACH(const Index& j, indices) {
const SubVector& vj = at(j);
size_t dj = (size_t) vj.rows();
result.segment(curHead, dj) = vj;
curHead += dj;
}
double VectorValues::dot(const VectorValues& v) const {
double result = 0.0;
if(this->size() != v.size())
throw invalid_argument("VectorValues::dot called with different vector sizes");
for(Index j = 0; j < this->size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
if(this->values_[j].size() == v.values_[j].size())
result += this->values_[j].dot(v.values_[j]);
else
throw invalid_argument("VectorValues::dot called with different vector sizes");
return result;
}
/* ************************************************************************* */
double VectorValues::norm() const {
return std::sqrt(this->squaredNorm());
}
/* ************************************************************************* */
double VectorValues::squaredNorm() const {
double sumSquares = 0.0;
for(Index j = 0; j < this->size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
sumSquares += this->values_[j].squaredNorm();
return sumSquares;
}
/* ************************************************************************* */
VectorValues VectorValues::operator+(const VectorValues& c) const {
VectorValues result = SameStructure(*this);
if(this->size() != c.size())
throw invalid_argument("VectorValues::operator+ called with different vector sizes");
for(Index j = 0; j < this->size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
if(this->values_[j].size() == c.values_[j].size())
result.values_[j] = this->values_[j] + c.values_[j];
else
throw invalid_argument("VectorValues::operator- called with different vector sizes");
return result;
}
/* ************************************************************************* */
VectorValues VectorValues::operator-(const VectorValues& c) const {
VectorValues result = SameStructure(*this);
if(this->size() != c.size())
throw invalid_argument("VectorValues::operator- called with different vector sizes");
for(Index j = 0; j < this->size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
if(this->values_[j].size() == c.values_[j].size())
result.values_[j] = this->values_[j] - c.values_[j];
else
throw invalid_argument("VectorValues::operator- called with different vector sizes");
return result;
}
/* ************************************************************************* */
void VectorValues::operator+=(const VectorValues& c) {
if(this->size() != c.size())
throw invalid_argument("VectorValues::operator+= called with different vector sizes");
for(Index j = 0; j < this->size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
if(this->values_[j].size() == c.values_[j].size())
this->values_[j] += c.values_[j];
else
throw invalid_argument("VectorValues::operator+= called with different vector sizes");
}
/* ************************************************************************* */
} // \namespace gtsam

306
gtsam/linear/VectorValues.h
View File

@ -93,15 +93,14 @@ namespace gtsam {
*/
class VectorValues {
protected:
Vector values_; ///< The underlying vector storing the values
typedef std::vector<SubVector> ValueMaps; ///< Collection of SubVector s
ValueMaps maps_; ///< SubVector s referencing each vector variable in values_
typedef std::vector<Vector> Values; ///< Typedef for the collection of Vectors making up a VectorValues
Values values_; ///< Collection of Vectors making up this VectorValues
public:
typedef ValueMaps::iterator iterator; ///< Iterator over vector values
typedef ValueMaps::const_iterator const_iterator; ///< Const iterator over vector values
typedef ValueMaps::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values
typedef ValueMaps::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values
typedef Values::iterator iterator; ///< Iterator over vector values
typedef Values::const_iterator const_iterator; ///< Const iterator over vector values
typedef Values::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values
typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values
typedef boost::shared_ptr<VectorValues> shared_ptr; ///< shared_ptr to this class
/// @name Standard Constructors
@ -112,11 +111,8 @@ namespace gtsam {
*/
VectorValues() {}
/** Copy constructor */
VectorValues(const VectorValues &other);
/** Named constructor to create a VectorValues of the same structure of the
* specifed one, but filled with zeros.
* specified one, but filled with zeros.
* @return
*/
static VectorValues Zero(const VectorValues& model);
@ -127,55 +123,49 @@ namespace gtsam {
/** Number of variables stored, always 1 more than the highest variable index,
* even if some variables with lower indices are not present. */
Index size() const { return maps_.size(); }
Index size() const { return values_.size(); }
/** Return the dimension of variable \c j. */
size_t dim(Index j) const { checkExists(j); return (*this)[j].rows(); }
/** Return the summed dimensionality of all variables. */
size_t dim() const { return values_.rows(); }
/** Return the dimension of each vector in this container */
std::vector<size_t> dims() const;
/** Check whether a variable with index \c j exists. */
bool exists(Index j) const { return j < size() && maps_[j].rows() > 0; }
bool exists(Index j) const { return j < size() && values_[j].rows() > 0; }
/** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to operator[](Index). */
SubVector& at(Index j) { checkExists(j); return maps_[j]; }
Vector& at(Index j) { checkExists(j); return values_[j]; }
/** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to operator[](Index). */
const SubVector& at(Index j) const { checkExists(j); return maps_[j]; }
const Vector& at(Index j) const { checkExists(j); return values_[j]; }
/** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to at(Index). */
SubVector& operator[](Index j) { return at(j); }
Vector& operator[](Index j) { return at(j); }
/** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to at(Index). */
const SubVector& operator[](Index j) const { return at(j); }
const Vector& operator[](Index j) const { return at(j); }
/** Insert a vector \c value with index \c j.
* Causes reallocation. Can be used to insert values in any order, but
* throws an invalid_argument exception if the index \c j is already used.
* Causes reallocation, but can insert values in any order.
* Throws an invalid_argument exception if the index \c j is already used.
* @param value The vector to be inserted.
* @param j The index with which the value will be associated.
*/
void insert(Index j, const Vector& value);
/** Assignment */
VectorValues& operator=(const VectorValues& rhs);
iterator begin() { chk(); return maps_.begin(); } ///< Iterator over variables
const_iterator begin() const { chk(); return maps_.begin(); } ///< Iterator over variables
iterator end() { chk(); return maps_.end(); } ///< Iterator over variables
const_iterator end() const { chk(); return maps_.end(); } ///< Iterator over variables
reverse_iterator rbegin() { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables
const_reverse_iterator rbegin() const { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables
reverse_iterator rend() { chk(); return maps_.rend(); } ///< Reverse iterator over variables
const_reverse_iterator rend() const { chk(); return maps_.rend(); } ///< Reverse iterator over variables
iterator begin() { return values_.begin(); } ///< Iterator over variables
const_iterator begin() const { return values_.begin(); } ///< Iterator over variables
iterator end() { return values_.end(); } ///< Iterator over variables
const_iterator end() const { return values_.end(); } ///< Iterator over variables
reverse_iterator rbegin() { return values_.rbegin(); } ///< Reverse iterator over variables
const_reverse_iterator rbegin() const { return values_.rbegin(); } ///< Reverse iterator over variables
reverse_iterator rend() { return values_.rend(); } ///< Reverse iterator over variables
const_reverse_iterator rend() const { return values_.rend(); } ///< Reverse iterator over variables
/** print required by Testable for unit testing */
void print(const std::string& str = "VectorValues: ",
const IndexFormatter& formatter =DefaultIndexFormatter) const;
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/** equals required by Testable for unit testing */
bool equals(const VectorValues& x, double tol = 1e-9) const;
@ -187,10 +177,10 @@ namespace gtsam {
/** Construct from a container of variable dimensions (in variable order), without initializing any values. */
template<class CONTAINER>
explicit VectorValues(const CONTAINER& dimensions) { append(dimensions); }
explicit VectorValues(const CONTAINER& dimensions) { this->append(dimensions); }
/** Construct to hold nVars vectors of varDim dimension each. */
VectorValues(Index nVars, size_t varDim) { resize(nVars, varDim); }
VectorValues(Index nVars, size_t varDim) { this->resize(nVars, varDim); }
/** Named constructor to create a VectorValues that matches the structure of
* the specified VectorValues, but do not initialize the new values. */
@ -222,17 +212,16 @@ namespace gtsam {
void resizeLike(const VectorValues& other);
/** Resize the VectorValues to hold \c nVars variables, each of dimension
* \c varDim, not preserving any data. After calling this function, all
* variables will be uninitialized.
* \c varDim. Any individual vectors that do not change size will keep
* their values, but any new or resized vectors will be uninitialized.
* @param nVars The number of variables to create
* @param varDim The dimension of each variable
*/
void resize(Index nVars, size_t varDim);
/** Resize the VectorValues to contain variables of the dimensions stored
* in \c dimensions, not preserving any data. The new variables are
* uninitialized, but this function is used to pre-allocate space for
* performance. After calling this function all variables will be uninitialized.
* in \c dimensions. Any individual vectors that do not change size will keep
* their values, but any new or resized vectors will be uninitialized.
* @param dimensions A container of the dimension of each variable to create.
*/
template<class CONTAINER>
@ -251,14 +240,11 @@ namespace gtsam {
/** Set all entries to zero, does not modify the size. */
void setZero();
/** Reference the entire solution vector (const version). */
const Vector& vector() const { chk(); return values_; }
/** Reference the entire solution vector. */
Vector& vector() { chk(); return values_; }
/** Retrieve the entire solution as a single vector */
const Vector asVector() const;
/** Access a vector that is a subset of relevant indices */
Vector vector(const std::vector<Index>& indices) const;
const Vector vector(const std::vector<Index>& indices) const;
/** Check whether this VectorValues has the same structure, meaning has the
* same number of variables and that all variables are of the same dimension,
@ -268,16 +254,34 @@ namespace gtsam {
*/
bool hasSameStructure(const VectorValues& other) const;
/**
* Permute the variables in the VariableIndex according to the given partial permutation
*/
void permuteInPlace(const Permutation& selector, const Permutation& permutation);
/**
* Permute the entries of this VectorValues in place
*/
void permuteInPlace(const Permutation& permutation);
/**
* Swap the data in this VectorValues with another.
*/
void swap(VectorValues& other);
/// @}
/// @name Linear algebra operations
/// @{
/** Dot product with another VectorValues, interpreting both as vectors of
* their concatenated values. */
double dot(const VectorValues& V) const {
return gtsam::dot(this->values_, V.values_);
}
double dot(const VectorValues& v) const;
/** Vector L2 norm */
inline double norm() const {
return this->vector().norm();
}
double norm() const;
/** Squared vector L2 norm */
double squaredNorm() const;
/**
* + operator does element-wise addition. Both VectorValues must have the
@ -297,26 +301,11 @@ namespace gtsam {
*/
void operator+=(const VectorValues& c);
/**
* Permute the entries of this VectorValues, returns a new VectorValues as
* the result.
*/
VectorValues permute(const Permutation& permutation) const;
/**
* Swap the data in this VectorValues with another.
*/
void swap(VectorValues& other);
/// @}
private:
// Verifies that the underlying Vector is consistent with the collection of SubVectors
void chk() const;
// Throw an exception if j does not exist
inline void checkExists(Index j) const {
chk();
void checkExists(Index j) const {
if(!exists(j)) {
const std::string msg =
(boost::format("VectorValues: requested variable index j=%1% is not in this VectorValues.") % j).str();
@ -329,73 +318,63 @@ namespace gtsam {
/**
* scale a vector by a scalar
*/
friend VectorValues operator*(const double a, const VectorValues &V) {
VectorValues result(VectorValues::SameStructure(V));
result.values_ = a * V.values_;
friend VectorValues operator*(const double a, const VectorValues &v) {
VectorValues result = VectorValues::SameStructure(v);
for(Index j = 0; j < v.size(); ++j)
result.values_[j] = a * v.values_[j];
return result;
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend size_t dim(const VectorValues& V) {
return V.dim();
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend double dot(const VectorValues& V1, const VectorValues& V2) {
return gtsam::dot(V1.values_, V2.values_);
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend void scal(double alpha, VectorValues& x) {
gtsam::scal(alpha, x.values_);
for(Index j = 0; j < x.size(); ++j)
x.values_[j] *= alpha;
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend void axpy(double alpha, const VectorValues& x, VectorValues& y) {
gtsam::axpy(alpha, x.values_, y.values_);
if(x.size() != y.size())
throw std::invalid_argument("axpy(VectorValues) called with different vector sizes");
for(Index j = 0; j < x.size(); ++j)
if(x.values_[j].size() == y.values_[j].size())
y.values_[j] += alpha * x.values_[j];
else
throw std::invalid_argument("axpy(VectorValues) called with different vector sizes");
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend void sqrt(VectorValues &x) {
Vector y = gtsam::esqrt(x.values_);
x.values_ = y;
for(Index j = 0; j < x.size(); ++j)
x.values_[j] = x.values_[j].cwiseSqrt();
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend void ediv(const VectorValues& numerator,
const VectorValues& denominator, VectorValues &result) {
assert(
numerator.dim() == denominator.dim() && denominator.dim() == result.dim());
const size_t sz = result.dim();
for (size_t i = 0; i < sz; ++i)
result.values_[i] = numerator.values_[i] / denominator.values_[i];
friend void ediv(const VectorValues& numerator, const VectorValues& denominator, VectorValues &result) {
if(numerator.size() != denominator.size() || numerator.size() != result.size())
throw std::invalid_argument("ediv(VectorValues) called with different vector sizes");
for(Index j = 0; j < numerator.size(); ++j)
if(numerator.values_[j].size() == denominator.values_[j].size() && numerator.values_[j].size() == result.values_[j].size())
result.values_[j] = numerator.values_[j].cwiseQuotient(denominator.values_[j]);
else
throw std::invalid_argument("ediv(VectorValues) called with different vector sizes");
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend void edivInPlace(VectorValues& x, const VectorValues& y) {
assert(x.dim() == y.dim());
const size_t sz = x.dim();
for (size_t i = 0; i < sz; ++i)
x.values_[i] /= y.values_[i];
if(x.size() != y.size())
throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes");
for(Index j = 0; j < x.size(); ++j)
if(x.values_[j].size() == y.values_[j].size())
x.values_[j].array() /= y.values_[j].array();
else
throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes");
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void save(ARCHIVE & ar, const unsigned int version) const {
// The maps_ stores pointers, so we serialize dimensions instead
std::vector<size_t> dimensions(size());
for(size_t j=0; j<maps_.size(); ++j)
dimensions[j] = maps_[j].rows();
ar & BOOST_SERIALIZATION_NVP(dimensions);
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(values_);
}
template<class ARCHIVE>
void load(ARCHIVE & ar, const unsigned int version) {
std::vector<size_t> dimensions;
ar & BOOST_SERIALIZATION_NVP(dimensions); // Load dimensions
resize(dimensions); // Allocate space for everything
ar & BOOST_SERIALIZATION_NVP(values_); // Load values
chk();
}
BOOST_SERIALIZATION_SPLIT_MEMBER()
}; // VectorValues definition
// Implementations of template and inline functions
@ -403,86 +382,75 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONTAINER>
void VectorValues::resize(const CONTAINER& dimensions) {
maps_.clear();
values_.resize(0);
values_.clear();
append(dimensions);
}
/* ************************************************************************* */
template<class CONTAINER>
void VectorValues::append(const CONTAINER& dimensions) {
chk();
int newDim = std::accumulate(dimensions.begin(), dimensions.end(), 0); // Sum of dimensions
values_.conservativeResize(dim() + newDim);
// Relocate existing maps
int varStart = 0;
for(size_t j = 0; j < maps_.size(); ++j) {
new (&maps_[j]) SubVector(values_.segment(varStart, maps_[j].rows()));
varStart += maps_[j].rows();
}
maps_.reserve(maps_.size() + dimensions.size());
size_t i = size();
values_.resize(size() + dimensions.size());
BOOST_FOREACH(size_t dim, dimensions) {
maps_.push_back(values_.segment(varStart, dim));
varStart += (int)dim; // varStart is continued from first for loop
values_[i] = Vector(dim);
++ i;
}
}
/* ************************************************************************* */
template<class CONTAINER>
VectorValues VectorValues::Zero(const CONTAINER& dimensions) {
VectorValues ret(dimensions);
ret.setZero();
return ret;
}
/* ************************************************************************* */
inline void VectorValues::chk() const {
#ifndef NDEBUG
// Check that the first SubVector points to the beginning of the Vector
if(maps_.size() > 0) {
assert(values_.data() == maps_[0].data());
// Check that the end of the last SubVector points to the end of the Vector
assert(values_.rows() == maps_.back().data() + maps_.back().rows() - maps_.front().data());
VectorValues ret;
ret.values_.resize(dimensions.size());
size_t i = 0;
BOOST_FOREACH(size_t dim, dimensions) {
ret.values_[i] = Vector::Zero(dim);
++ i;
}
#endif
return ret;
}
namespace internal {
/* ************************************************************************* */
// Helper function, extracts vectors with variable indices
// in the first and last iterators, and concatenates them in that order into the
// output.
template<class VALUES, typename ITERATOR>
Vector extractVectorValuesSlices(const VALUES& values, ITERATOR first, ITERATOR last) {
// Find total dimensionality
int dim = 0;
for(ITERATOR j = first; j != last; ++j)
dim += values[*j].rows();
/* ************************************************************************* */
// Helper function, extracts vectors with variable indices
// in the first and last iterators, and concatenates them in that order into the
// output.
template<typename ITERATOR>
const Vector extractVectorValuesSlices(const VectorValues& values, ITERATOR first, ITERATOR last, bool allowNonexistant = false) {
// Find total dimensionality
int dim = 0;
for(ITERATOR j = first; j != last; ++j)
// If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent)
if(!allowNonexistant || values.exists(*j))
dim += values.dim(*j);
// Copy vectors
Vector ret(dim);
int varStart = 0;
for(ITERATOR j = first; j != last; ++j) {
ret.segment(varStart, values[*j].rows()) = values[*j];
varStart += values[*j].rows();
// Copy vectors
Vector ret(dim);
int varStart = 0;
for(ITERATOR j = first; j != last; ++j) {
// If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent)
if(!allowNonexistant || values.exists(*j)) {
ret.segment(varStart, values.dim(*j)) = values[*j];
varStart += values.dim(*j);
}
}
return ret;
}
return ret;
}
/* ************************************************************************* */
// Helper function, writes to the variables in values
// with indices iterated over by first and last, interpreting vector as the
// concatenated vectors to write.
template<class VECTOR, class VALUES, typename ITERATOR>
void writeVectorValuesSlices(const VECTOR& vector, VALUES& values, ITERATOR first, ITERATOR last) {
// Copy vectors
int varStart = 0;
for(ITERATOR j = first; j != last; ++j) {
values[*j] = vector.segment(varStart, values[*j].rows());
varStart += values[*j].rows();
/* ************************************************************************* */
// Helper function, writes to the variables in values
// with indices iterated over by first and last, interpreting vector as the
// concatenated vectors to write.
template<class VECTOR, typename ITERATOR>
void writeVectorValuesSlices(const VECTOR& vector, VectorValues& values, ITERATOR first, ITERATOR last) {
// Copy vectors
int varStart = 0;
for(ITERATOR j = first; j != last; ++j) {
values[*j] = vector.segment(varStart, values[*j].rows());
varStart += values[*j].rows();
}
assert(varStart == vector.rows());
}
assert(varStart == vector.rows());
}
}
} // \namespace gtsam

27
gtsam/linear/tests/testVectorValues.cpp
View File

@ -39,7 +39,6 @@ TEST(VectorValues, insert) {
// Check dimensions
LONGS_EQUAL(6, actual.size());
LONGS_EQUAL(7, actual.dim());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
@ -59,7 +58,7 @@ TEST(VectorValues, insert) {
EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5]));
EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.vector()));
EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector()));
// Check exceptions
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
@ -80,7 +79,6 @@ TEST(VectorValues, dimsConstructor) {
// Check dimensions
LONGS_EQUAL(3, actual.size());
LONGS_EQUAL(5, actual.dim());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
@ -89,7 +87,7 @@ TEST(VectorValues, dimsConstructor) {
EXPECT(assert_equal(Vector_(1, 1.0), actual[0]));
EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.vector()));
EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.asVector()));
}
/* ************************************************************************* */
@ -106,7 +104,6 @@ TEST(VectorValues, copyConstructor) {
// Check dimensions
LONGS_EQUAL(6, actual.size());
LONGS_EQUAL(7, actual.dim());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
@ -126,7 +123,7 @@ TEST(VectorValues, copyConstructor) {
EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5]));
EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.vector()));
EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector()));
// Check exceptions
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
@ -149,7 +146,6 @@ TEST(VectorValues, assignment) {
// Check dimensions
LONGS_EQUAL(6, actual.size());
LONGS_EQUAL(7, actual.dim());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
@ -169,7 +165,7 @@ TEST(VectorValues, assignment) {
EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5]));
EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.vector()));
EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector()));
// Check exceptions
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
@ -188,7 +184,6 @@ TEST(VectorValues, SameStructure) {
// Check dimensions
LONGS_EQUAL(6, actual.size());
LONGS_EQUAL(7, actual.dim());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
@ -220,7 +215,6 @@ TEST(VectorValues, Zero_fromModel) {
// Check dimensions
LONGS_EQUAL(6, actual.size());
LONGS_EQUAL(7, actual.dim());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
@ -256,7 +250,6 @@ TEST(VectorValues, Zero_fromDims) {
// Check dimensions
LONGS_EQUAL(3, actual.size());
LONGS_EQUAL(5, actual.dim());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
@ -273,7 +266,6 @@ TEST(VectorValues, Zero_fromUniform) {
// Check dimensions
LONGS_EQUAL(3, actual.size());
LONGS_EQUAL(6, actual.dim());
LONGS_EQUAL(2, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
@ -298,7 +290,6 @@ TEST(VectorValues, resizeLike) {
// Check dimensions
LONGS_EQUAL(6, actual.size());
LONGS_EQUAL(7, actual.dim());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
@ -328,7 +319,6 @@ TEST(VectorValues, resize_fromUniform) {
// Check dimensions
LONGS_EQUAL(3, actual.size());
LONGS_EQUAL(6, actual.dim());
LONGS_EQUAL(2, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
@ -337,7 +327,7 @@ TEST(VectorValues, resize_fromUniform) {
EXPECT(assert_equal(Vector_(2, 1.0, 2.0), actual[0]));
EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
EXPECT(assert_equal(Vector_(6, 1.0, 2.0, 2.0, 3.0, 4.0, 5.0), actual.vector()));
EXPECT(assert_equal(Vector_(6, 1.0, 2.0, 2.0, 3.0, 4.0, 5.0), actual.asVector()));
}
/* ************************************************************************* */
@ -355,7 +345,6 @@ TEST(VectorValues, resize_fromDims) {
// Check dimensions
LONGS_EQUAL(3, actual.size());
LONGS_EQUAL(5, actual.dim());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
@ -364,7 +353,7 @@ TEST(VectorValues, resize_fromDims) {
EXPECT(assert_equal(Vector_(1, 1.0), actual[0]));
EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.vector()));
EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.asVector()));
}
/* ************************************************************************* */
@ -383,7 +372,6 @@ TEST(VectorValues, append) {
// Check dimensions
LONGS_EQUAL(5, actual.size());
LONGS_EQUAL(13, actual.dim());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
@ -443,7 +431,8 @@ TEST(VectorValues, permute) {
permutation[2] = 3;
permutation[3] = 1;
VectorValues actual = original.permute(permutation);
VectorValues actual = original;
actual.permuteInPlace(permutation);
EXPECT(assert_equal(expected, actual));
}

10
gtsam/nonlinear/DoglegOptimizerImpl.cpp
View File

@ -28,13 +28,12 @@ VectorValues DoglegOptimizerImpl::ComputeDoglegPoint(
// Get magnitude of each update and find out which segment Delta falls in
assert(Delta >= 0.0);
double DeltaSq = Delta*Delta;
double x_u_norm_sq = dx_u.vector().squaredNorm();
double x_n_norm_sq = dx_n.vector().squaredNorm();
double x_u_norm_sq = dx_u.squaredNorm();
double x_n_norm_sq = dx_n.squaredNorm();
if(verbose) cout << "Steepest descent magnitude " << std::sqrt(x_u_norm_sq) << ", Newton's method magnitude " << std::sqrt(x_n_norm_sq) << endl;
if(DeltaSq < x_u_norm_sq) {
// Trust region is smaller than steepest descent update
VectorValues x_d = VectorValues::SameStructure(dx_u);
x_d.vector() = dx_u.vector() * std::sqrt(DeltaSq / x_u_norm_sq);
VectorValues x_d = std::sqrt(DeltaSq / x_u_norm_sq) * dx_u;
if(verbose) cout << "In steepest descent region with fraction " << std::sqrt(DeltaSq / x_u_norm_sq) << " of steepest descent magnitude" << endl;
return x_d;
} else if(DeltaSq < x_n_norm_sq) {
@ -79,8 +78,7 @@ VectorValues DoglegOptimizerImpl::ComputeBlend(double Delta, const VectorValues&
// Compute blended point
if(verbose) cout << "In blend region with fraction " << tau << " of Newton's method point" << endl;
VectorValues blend = VectorValues::SameStructure(x_u);
blend.vector() = (1. - tau) * x_u.vector() + tau * x_n.vector();
VectorValues blend = (1. - tau) * x_u; axpy(tau, x_n, blend);
return blend;
}

4
gtsam/nonlinear/DoglegOptimizerImpl.h
View File

@ -177,7 +177,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
result.dx_d = ComputeDoglegPoint(Delta, dx_u, dx_n, verbose);
gttoc(Dog_leg_point);
if(verbose) std::cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.vector().norm() << std::endl;
if(verbose) std::cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.norm() << std::endl;
gttic(retract);
// Compute expmapped solution
@ -208,7 +208,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
if(rho >= 0.75) {
// M agrees very well with f, so try to increase lambda
const double dx_d_norm = result.dx_d.vector().norm();
const double dx_d_norm = result.dx_d.norm();
const double newDelta = std::max(Delta, 3.0 * dx_d_norm); // Compute new Delta
if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY)

39
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -39,15 +39,15 @@ void ISAM2::Impl::AddVariables(
// Add the new keys onto the ordering, add zeros to the delta for the new variables
std::vector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary()));
if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl;
const size_t newDim = accumulate(dims.begin(), dims.end(), 0);
const size_t originalDim = delta.dim();
const size_t originalnVars = delta.size();
delta.append(dims);
delta.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
deltaNewton.append(dims);
deltaNewton.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
RgProd.append(dims);
RgProd.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
for(Index j = originalnVars; j < delta.size(); ++j) {
delta[j].setZero();
deltaNewton[j].setZero();
RgProd[j].setZero();
}
{
Index nextVar = originalnVars;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
@ -55,10 +55,9 @@ void ISAM2::Impl::AddVariables(
if(debug) cout << "Adding variable " << keyFormatter(key_value.key) << " with order " << nextVar << endl;
++ nextVar;
}
assert(ordering.nVars() == delta.size());
assert(ordering.size() == delta.size());
}
replacedKeys.resize(ordering.nVars(), false);
replacedKeys.resize(ordering.size(), false);
}
/* ************************************************************************* */
@ -115,9 +114,10 @@ void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const ISAM2Cli
}
// Reorder and remove from ordering and solution
ordering.permuteWithInverse(unusedToEndInverse);
ordering.permuteInPlace(unusedToEnd);
BOOST_REVERSE_FOREACH(Key key, unusedKeys) {
ordering.pop_back(key);
Ordering::value_type removed = ordering.pop_back();
assert(removed.first == key);
theta.erase(key);
}
@ -188,14 +188,14 @@ void CheckRelinearizationRecursiveDouble(FastSet<Index>& relinKeys, double thres
}
/* ************************************************************************* */
void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<char,Vector>& thresholds, const VectorValues& delta, const Ordering::InvertedMap& decoder, const ISAM2Clique::shared_ptr& clique) {
void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<char,Vector>& thresholds, const VectorValues& delta, const Ordering& ordering, const ISAM2Clique::shared_ptr& clique) {
// Check the current clique for relinearization
bool relinearize = false;
BOOST_FOREACH(Index var, clique->conditional()->keys()) {
// Lookup the key associated with this index
Key key = decoder.at(var);
Key key = ordering.key(var);
// Find the threshold for this variable type
const Vector& threshold = thresholds.find(Symbol(key).chr())->second;
@ -214,7 +214,7 @@ void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<c
// If this node was relinearized, also check its children
if(relinearize) {
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) {
CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, decoder, child);
CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, ordering, child);
}
}
}
@ -229,8 +229,7 @@ FastSet<Index> ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::share
if(relinearizeThreshold.type() == typeid(double)) {
CheckRelinearizationRecursiveDouble(relinKeys, boost::get<double>(relinearizeThreshold), delta, root);
} else if(relinearizeThreshold.type() == typeid(FastMap<char,Vector>)) {
Ordering::InvertedMap decoder = ordering.invert();
CheckRelinearizationRecursiveMap(relinKeys, boost::get<FastMap<char,Vector> >(relinearizeThreshold), delta, decoder, root);
CheckRelinearizationRecursiveMap(relinKeys, boost::get<FastMap<char,Vector> >(relinearizeThreshold), delta, ordering, root);
}
}
@ -267,8 +266,8 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, const
invalidateIfDebug = boost::none;
#endif
assert(values.size() == ordering.nVars());
assert(delta.size() == ordering.nVars());
assert(values.size() == ordering.size());
assert(delta.size() == ordering.size());
Values::iterator key_value;
Ordering::const_iterator key_index;
for(key_value = values.begin(), key_index = ordering.begin();
@ -353,11 +352,9 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
Permutation::shared_ptr affectedColamdInverse(affectedColamd->inverse());
if(debug) affectedColamd->print("affectedColamd: ");
if(debug) affectedColamdInverse->print("affectedColamdInverse: ");
result.fullReordering =
*Permutation::Identity(reorderingMode.nFullSystemVars).partialPermutation(affectedKeysSelector, *affectedColamd);
result.fullReorderingInverse =
*Permutation::Identity(reorderingMode.nFullSystemVars).partialPermutation(affectedKeysSelector, *affectedColamdInverse);
if(debug) result.fullReordering.print("partialReordering: ");
result.reorderingSelector = affectedKeysSelector;
result.reorderingPermutation = *affectedColamd;
result.reorderingInverse = internal::Reduction::CreateFromPartialPermutation(affectedKeysSelector, *affectedColamdInverse);
gttoc(ccolamd_permutations);
gttic(permute_affected_variable_index);

5
gtsam/nonlinear/ISAM2-impl.h
View File

@ -26,8 +26,9 @@ struct ISAM2::Impl {
struct PartialSolveResult {
ISAM2::sharedClique bayesTree;
Permutation fullReordering;
Permutation fullReorderingInverse;
Permutation reorderingSelector;
Permutation reorderingPermutation;
internal::Reduction reorderingInverse;
};
struct ReorderingMode {

4
gtsam/nonlinear/ISAM2-inl.h
View File

@ -29,7 +29,7 @@ namespace gtsam {
template<class VALUE>
VALUE ISAM2::calculateEstimate(Key key) const {
const Index index = getOrdering()[key];
const SubVector delta = getDelta()[index];
const Vector& delta = getDelta()[index];
return theta_.at<VALUE>(key).retract(delta);
}
@ -158,7 +158,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
if(!valuesChanged) {
const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]);
const SubVector& newValue(delta[*it]);
const Vector& newValue(delta[*it]);
if((oldValue - newValue).lpNorm<Eigen::Infinity>() >= threshold) {
valuesChanged = true;
break;

49
gtsam/nonlinear/ISAM2.cpp
View File

@ -348,12 +348,12 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
variableIndex_.permuteInPlace(*colamd);
gttoc(permute_global_variable_index);
gttic(permute_delta);
delta_ = delta_.permute(*colamd);
deltaNewton_ = deltaNewton_.permute(*colamd);
RgProd_ = RgProd_.permute(*colamd);
delta_.permuteInPlace(*colamd);
deltaNewton_.permuteInPlace(*colamd);
RgProd_.permuteInPlace(*colamd);
gttoc(permute_delta);
gttic(permute_ordering);
ordering_.permuteWithInverse(*colamdInverse);
ordering_.permuteInPlace(*colamd);
gttoc(permute_ordering);
gttoc(reorder);
@ -412,7 +412,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
// Reeliminated keys for detailed results
if(params_.enableDetailedResults) {
BOOST_FOREACH(Index index, affectedAndNewKeys) {
result.detail->variableStatus[inverseOrdering_->at(index)].isReeliminated = true;
result.detail->variableStatus[ordering_.key(index)].isReeliminated = true;
}
}
@ -450,7 +450,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
gttic(PartialSolve);
Impl::ReorderingMode reorderingMode;
reorderingMode.nFullSystemVars = ordering_.nVars();
reorderingMode.nFullSystemVars = ordering_.size();
reorderingMode.algorithm = Impl::ReorderingMode::COLAMD;
reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST;
if(constrainKeys) {
@ -481,22 +481,22 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
// re-eliminate. The reordered variables are also mentioned in the
// orphans and the leftover cached factors.
gttic(permute_global_variable_index);
variableIndex_.permuteInPlace(partialSolveResult.fullReordering);
variableIndex_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
gttoc(permute_global_variable_index);
gttic(permute_delta);
delta_ = delta_.permute(partialSolveResult.fullReordering);
deltaNewton_ = deltaNewton_.permute(partialSolveResult.fullReordering);
RgProd_ = RgProd_.permute(partialSolveResult.fullReordering);
delta_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
deltaNewton_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
RgProd_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
gttoc(permute_delta);
gttic(permute_ordering);
ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
ordering_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
gttoc(permute_ordering);
if(params_.cacheLinearizedFactors) {
gttic(permute_cached_linear);
//linearFactors_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
FastList<size_t> permuteLinearIndices = getAffectedFactors(affectedAndNewKeys);
BOOST_FOREACH(size_t idx, permuteLinearIndices) {
linearFactors_[idx]->permuteWithInverse(partialSolveResult.fullReorderingInverse);
linearFactors_[idx]->reduceWithInverse(partialSolveResult.reorderingInverse);
}
gttoc(permute_cached_linear);
}
@ -514,7 +514,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
gttic(orphans);
gttic(permute);
BOOST_FOREACH(sharedClique orphan, orphans) {
(void)orphan->permuteSeparatorWithInverse(partialSolveResult.fullReorderingInverse);
(void)orphan->reduceSeparatorWithInverse(partialSolveResult.reorderingInverse);
}
gttoc(permute);
gttic(insert);
@ -538,7 +538,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
// Root clique variables for detailed results
if(params_.enableDetailedResults) {
BOOST_FOREACH(Index index, this->root()->conditional()->frontals()) {
result.detail->variableStatus[inverseOrdering_->at(index)].inRootClique = true;
result.detail->variableStatus[ordering_.key(index)].inRootClique = true;
}
}
@ -628,7 +628,6 @@ ISAM2Result ISAM2::update(
Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_);
// New keys for detailed results
if(params_.enableDetailedResults) {
inverseOrdering_ = ordering_.invert();
BOOST_FOREACH(Key key, newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } }
gttoc(add_new_variables);
@ -649,7 +648,7 @@ ISAM2Result ISAM2::update(
// Observed keys for detailed results
if(params_.enableDetailedResults) {
BOOST_FOREACH(Index index, markedKeys) {
result.detail->variableStatus[inverseOrdering_->at(index)].isObserved = true;
result.detail->variableStatus[ordering_.key(index)].isObserved = true;
}
}
// NOTE: we use assign instead of the iterator constructor here because this
@ -666,7 +665,7 @@ ISAM2Result ISAM2::update(
FastSet<Index> relinKeys;
if (relinearizeThisStep) {
gttic(gather_relinearize_keys);
vector<bool> markedRelinMask(ordering_.nVars(), false);
vector<bool> markedRelinMask(ordering_.size(), false);
// 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
if(params_.enablePartialRelinearizationCheck)
relinKeys = Impl::CheckRelinearizationPartial(root_, delta_, ordering_, params_.relinearizeThreshold);
@ -677,8 +676,8 @@ ISAM2Result ISAM2::update(
// Above relin threshold keys for detailed results
if(params_.enableDetailedResults) {
BOOST_FOREACH(Index index, relinKeys) {
result.detail->variableStatus[inverseOrdering_->at(index)].isAboveRelinThreshold = true;
result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearized = true; } }
result.detail->variableStatus[ordering_.key(index)].isAboveRelinThreshold = true;
result.detail->variableStatus[ordering_.key(index)].isRelinearized = true; } }
// Add the variables being relinearized to the marked keys
BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; }
@ -696,9 +695,9 @@ ISAM2Result ISAM2::update(
FastSet<Index> involvedRelinKeys;
Impl::FindAll(this->root(), involvedRelinKeys, markedRelinMask);
BOOST_FOREACH(Index index, involvedRelinKeys) {
if(!result.detail->variableStatus[inverseOrdering_->at(index)].isAboveRelinThreshold) {
result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearizeInvolved = true;
result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearized = true; } }
if(!result.detail->variableStatus[ordering_.key(index)].isAboveRelinThreshold) {
result.detail->variableStatus[ordering_.key(index)].isRelinearizeInvolved = true;
result.detail->variableStatus[ordering_.key(index)].isRelinearized = true; } }
}
}
gttoc(fluid_find_all);
@ -818,7 +817,7 @@ Values ISAM2::calculateEstimate() const {
const VectorValues& delta(getDelta());
gttoc(getDelta);
gttic(Expmap);
vector<bool> mask(ordering_.nVars(), true);
vector<bool> mask(ordering_.size(), true);
Impl::ExpmapMasked(ret, delta, ordering_, mask);
gttoc(Expmap);
return ret;
@ -905,13 +904,13 @@ void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) {
gttic(Compute_minimizing_step_size);
// Compute minimizing step size
double RgNormSq = isam.RgProd_.vector().squaredNorm();
double RgNormSq = isam.RgProd_.asVector().squaredNorm();
double step = -gradientSqNorm / RgNormSq;
gttoc(Compute_minimizing_step_size);
gttic(Compute_point);
// Compute steepest descent point
grad.vector() *= step;
scal(step, grad);
gttoc(Compute_point);
}

15
gtsam/nonlinear/ISAM2.h
View File

@ -405,9 +405,15 @@ public:
Base::permuteWithInverse(inversePermutation);
}
bool permuteSeparatorWithInverse(const Permutation& inversePermutation) {
bool changed = Base::permuteSeparatorWithInverse(inversePermutation);
if(changed) if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
//bool permuteSeparatorWithInverse(const Permutation& inversePermutation) {
// bool changed = Base::permuteSeparatorWithInverse(inversePermutation);
// if(changed) if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
// return changed;
//}
bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) {
bool changed = Base::reduceSeparatorWithInverse(inverseReduction);
if(changed) if(cachedFactor_) cachedFactor_->reduceWithInverse(inverseReduction);
return changed;
}
@ -496,9 +502,6 @@ protected:
/** The current Dogleg Delta (trust region radius) */
mutable boost::optional<double> doglegDelta_;
/** The inverse ordering, only used for creating ISAM2Result::DetailedResults */
boost::optional<Ordering::InvertedMap> inverseOrdering_;
public:
typedef ISAM2 This; ///< This class

2
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -110,7 +110,7 @@ void LevenbergMarquardtOptimizer::iterate() {
// Solve Damped Gaussian Factor Graph
const VectorValues delta = solveGaussianFactorGraph(dampedSystem, params_);
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.vector().norm() << endl;
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.norm() << endl;
if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta");
// update values

53
gtsam/nonlinear/LinearContainerFactor.cpp
View File

@ -13,14 +13,8 @@ namespace gtsam {
/* ************************************************************************* */
void LinearContainerFactor::rekeyFactor(const Ordering& ordering) {
Ordering::InvertedMap invOrdering = ordering.invert(); // TODO: inefficient - make more selective ordering invert
rekeyFactor(invOrdering);
}
/* ************************************************************************* */
void LinearContainerFactor::rekeyFactor(const Ordering::InvertedMap& invOrdering) {
BOOST_FOREACH(Index& idx, factor_->keys()) {
Key fullKey = invOrdering.find(idx)->second;
Key fullKey = ordering.key(idx);
idx = fullKey;
keys_.push_back(fullKey);
}
@ -86,34 +80,6 @@ LinearContainerFactor::LinearContainerFactor(
initializeLinearizationPoint(linearizationPoint);
}
/* ************************************************************************* */
LinearContainerFactor::LinearContainerFactor(const JacobianFactor& factor,
const Ordering::InvertedMap& inverted_ordering,
const Values& linearizationPoint)
: factor_(factor.clone()) {
rekeyFactor(inverted_ordering);
initializeLinearizationPoint(linearizationPoint);
}
/* ************************************************************************* */
LinearContainerFactor::LinearContainerFactor(const HessianFactor& factor,
const Ordering::InvertedMap& inverted_ordering,
const Values& linearizationPoint)
: factor_(factor.clone()) {
rekeyFactor(inverted_ordering);
initializeLinearizationPoint(linearizationPoint);
}
/* ************************************************************************* */
LinearContainerFactor::LinearContainerFactor(
const GaussianFactor::shared_ptr& factor,
const Ordering::InvertedMap& ordering,
const Values& linearizationPoint)
: factor_(factor->clone()) {
rekeyFactor(ordering);
initializeLinearizationPoint(linearizationPoint);
}
/* ************************************************************************* */
void LinearContainerFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
Base::print(s+"LinearContainerFactor", keyFormatter);
@ -198,6 +164,7 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(
// Determine delta between linearization points using new ordering
VectorValues delta = linearizationPoint_->localCoordinates(subsetC, localOrdering);
Vector deltaVector = delta.asVector();
// clone and reorder linear factor to final ordering
GaussianFactor::shared_ptr linFactor = order(localOrdering);
@ -208,15 +175,14 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(
HessianFactor::shared_ptr hesFactor = boost::shared_dynamic_cast<HessianFactor>(linFactor);
size_t dim = hesFactor->linearTerm().size();
Eigen::Block<HessianFactor::Block> Gview = hesFactor->info().block(0, 0, dim, dim);
Vector G_delta = Gview.selfadjointView<Eigen::Upper>() * delta.vector();
hesFactor->constantTerm() += delta.vector().dot(G_delta) + delta.vector().dot(hesFactor->linearTerm());
Vector G_delta = Gview.selfadjointView<Eigen::Upper>() * deltaVector;
hesFactor->constantTerm() += deltaVector.dot(G_delta) + deltaVector.dot(hesFactor->linearTerm());
hesFactor->linearTerm() += G_delta;
}
// reset ordering
Ordering::InvertedMap invLocalOrdering = localOrdering.invert();
BOOST_FOREACH(Index& idx, linFactor->keys())
idx = ordering[invLocalOrdering[idx] ];
idx = ordering[localOrdering.key(idx)];
return linFactor;
}
@ -258,18 +224,11 @@ NonlinearFactor::shared_ptr LinearContainerFactor::negate() const {
NonlinearFactorGraph LinearContainerFactor::convertLinearGraph(
const GaussianFactorGraph& linear_graph, const Ordering& ordering,
const Values& linearizationPoint) {
return convertLinearGraph(linear_graph, ordering.invert());
}
/* ************************************************************************* */
NonlinearFactorGraph LinearContainerFactor::convertLinearGraph(
const GaussianFactorGraph& linear_graph, const InvertedOrdering& invOrdering,
const Values& linearizationPoint) {
NonlinearFactorGraph result;
BOOST_FOREACH(const GaussianFactor::shared_ptr& f, linear_graph)
if (f)
result.push_back(NonlinearFactorGraph::sharedFactor(
new LinearContainerFactor(f, invOrdering, linearizationPoint)));
new LinearContainerFactor(f, ordering, linearizationPoint)));
return result;
}

23
gtsam/nonlinear/LinearContainerFactor.h
View File

@ -45,21 +45,6 @@ public:
LinearContainerFactor(const GaussianFactor::shared_ptr& factor,
const Values& linearizationPoint = Values());
/** Alternate constructor: store a linear factor and decode keys with inverted ordering*/
LinearContainerFactor(const JacobianFactor& factor,
const Ordering::InvertedMap& inverted_ordering,
const Values& linearizationPoint = Values());
/** Alternate constructor: store a linear factor and decode keys with inverted ordering*/
LinearContainerFactor(const HessianFactor& factor,
const Ordering::InvertedMap& inverted_ordering,
const Values& linearizationPoint = Values());
/** Constructor from shared_ptr with inverted ordering*/
LinearContainerFactor(const GaussianFactor::shared_ptr& factor,
const Ordering::InvertedMap& ordering,
const Values& linearizationPoint = Values());
// Access
const GaussianFactor::shared_ptr& factor() const { return factor_; }
@ -130,19 +115,13 @@ public:
/**
* Utility function for converting linear graphs to nonlinear graphs
* consisting of LinearContainerFactors. Two versions are available, using
* either the ordering the linear graph was linearized around, or the inverse ordering.
* If the inverse ordering is present, it will be faster.
* consisting of LinearContainerFactors.
*/
static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph,
const Ordering& ordering, const Values& linearizationPoint = Values());
static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph,
const InvertedOrdering& invOrdering, const Values& linearizationPoint = Values());
protected:
void rekeyFactor(const Ordering& ordering);
void rekeyFactor(const Ordering::InvertedMap& invOrdering);
void initializeLinearizationPoint(const Values& linearizationPoint);
}; // \class LinearContainerFactor

2
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp
View File

@ -44,7 +44,7 @@ NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradient
}
NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(const State &current, const double alpha, const Gradient &g) const {
Gradient step = g;
step.vector() *= alpha;
scal(alpha, step);
return current.retract(step, ordering_);
}

4
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -223,7 +223,7 @@ Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(
variableIndex));
// Permute the Ordering with the COLAMD ordering
ordering->permuteWithInverse(*colamdPerm->inverse());
ordering->permuteInPlace(*colamdPerm);
return ordering;
}
@ -254,7 +254,7 @@ Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMDConstrained(const Value
variableIndex, index_constraints));
// Permute the Ordering with the COLAMD ordering
ordering->permuteWithInverse(*colamdPerm->inverse());
ordering->permuteInPlace(*colamdPerm);
return ordering;
}

6
gtsam/nonlinear/NonlinearISAM.cpp
View File

@ -55,10 +55,8 @@ void NonlinearISAM::update(const NonlinearFactorGraph& newFactors,
// Augment ordering
// TODO: allow for ordering constraints within the new variables
// FIXME: should just loop over new values
BOOST_FOREACH(const NonlinearFactorGraph::sharedFactor& factor, newFactors)
BOOST_FOREACH(Key key, factor->keys())
ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialValues)
ordering_.insert(key_value.key, ordering_.size());
boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors = newFactors.linearize(linPoint_, ordering_);

94
gtsam/nonlinear/Ordering.cpp
View File

@ -26,17 +26,53 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
Ordering::Ordering(const std::list<Key> & L):nVars_(0) {
Ordering::Ordering(const std::list<Key> & L) {
int i = 0;
BOOST_FOREACH( Key s, L )
insert(s, i++) ;
}
/* ************************************************************************* */
void Ordering::permuteWithInverse(const Permutation& inversePermutation) {
gttic(Ordering_permuteWithInverse);
BOOST_FOREACH(Ordering::value_type& key_order, *this) {
key_order.second = inversePermutation[key_order.second];
Ordering::Ordering(const Ordering& other) : order_(other.order_), orderingIndex_(other.size()) {
for(iterator item = order_.begin(); item != order_.end(); ++item)
orderingIndex_[item->second] = item;
}
/* ************************************************************************* */
Ordering& Ordering::operator=(const Ordering& rhs) {
order_ = rhs.order_;
orderingIndex_.resize(rhs.size());
for(iterator item = order_.begin(); item != order_.end(); ++item)
orderingIndex_[item->second] = item;
return *this;
}
/* ************************************************************************* */
void Ordering::permuteInPlace(const Permutation& permutation) {
gttic(Ordering_permuteInPlace);
// Allocate new index and permute in map iterators
OrderingIndex newIndex(permutation.size());
for(size_t j = 0; j < newIndex.size(); ++j) {
newIndex[j] = orderingIndex_[permutation[j]]; // Assign the iterator
newIndex[j]->second = j; // Change the stored index to its permuted value
}
// Swap the new index into this Ordering class
orderingIndex_.swap(newIndex);
}
/* ************************************************************************* */
void Ordering::permuteInPlace(const Permutation& selector, const Permutation& permutation) {
if(selector.size() != permutation.size())
throw invalid_argument("Ordering::permuteInPlace (partial permutation version) called with selector and permutation of different sizes.");
// Create new index the size of the permuted entries
OrderingIndex newIndex(selector.size());
// Permute the affected entries into the new index
for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot)
newIndex[dstSlot] = orderingIndex_[selector[permutation[dstSlot]]];
// Put the affected entries back in the new order and fix the indices
for(size_t slot = 0; slot < selector.size(); ++slot) {
orderingIndex_[selector[slot]] = newIndex[slot];
orderingIndex_[selector[slot]]->second = selector[slot];
}
}
@ -44,16 +80,15 @@ void Ordering::permuteWithInverse(const Permutation& inversePermutation) {
void Ordering::print(const string& str, const KeyFormatter& keyFormatter) const {
cout << str;
// Print ordering in index order
Ordering::InvertedMap inverted = this->invert();
// Print the ordering with varsPerLine ordering entries printed on each line,
// for compactness.
static const size_t varsPerLine = 10;
bool endedOnNewline = false;
BOOST_FOREACH(const Ordering::InvertedMap::value_type& index_key, inverted) {
if(index_key.first % varsPerLine != 0)
BOOST_FOREACH(const Map::iterator& index_key, orderingIndex_) {
if(index_key->second % varsPerLine != 0)
cout << ", ";
cout << index_key.first << ":" << keyFormatter(index_key.second);
if(index_key.first % varsPerLine == varsPerLine - 1) {
cout << index_key->second<< ":" << keyFormatter(index_key->first);
if(index_key->second % varsPerLine == varsPerLine - 1) {
cout << "\n";
endedOnNewline = true;
} else {
@ -72,40 +107,11 @@ bool Ordering::equals(const Ordering& rhs, double tol) const {
/* ************************************************************************* */
Ordering::value_type Ordering::pop_back() {
// FIXME: is there a way of doing this without searching over the entire structure?
for (iterator it=begin(); it!=end(); ++it) {
if (it->second == nVars_ - 1) {
value_type result = *it;
order_.erase(it);
--nVars_;
return result;
}
}
return value_type();
}
/* ************************************************************************* */
Index Ordering::pop_back(Key key) {
Map::iterator item = order_.find(key);
if(item == order_.end()) {
throw invalid_argument("Attempting to remove a key from an ordering that does not contain that key");
} else {
if(item->second != nVars_ - 1) {
throw invalid_argument("Attempting to remove a key from an ordering in which that key is not last");
} else {
order_.erase(item);
-- nVars_;
return nVars_;
}
}
}
/* ************************************************************************* */
Ordering::InvertedMap Ordering::invert() const {
InvertedMap result;
BOOST_FOREACH(const value_type& p, *this)
result.insert(make_pair(p.second, p.first));
return result;
iterator lastItem = orderingIndex_.back(); // Get the map iterator to the highest-index entry
value_type removed = *lastItem; // Save the key-index pair to return
order_.erase(lastItem); // Erase the entry from the map
orderingIndex_.pop_back(); // Erase the entry from the index
return removed; // Return the removed item
}
/* ************************************************************************* */

131
gtsam/nonlinear/Ordering.h
View File

@ -24,7 +24,7 @@
#include <boost/foreach.hpp>
#include <boost/assign/list_inserter.hpp>
#include <boost/pool/pool_alloc.hpp>
#include <set>
#include <vector>
namespace gtsam {
@ -35,8 +35,9 @@ namespace gtsam {
class Ordering {
protected:
typedef FastMap<Key, Index> Map;
typedef std::vector<Map::iterator> OrderingIndex;
Map order_;
Index nVars_;
OrderingIndex orderingIndex_;
public:
@ -51,25 +52,33 @@ public:
/// @{
/// Default constructor for empty ordering
Ordering() : nVars_(0) {}
Ordering() {}
/// Copy constructor
Ordering(const Ordering& other);
/// Construct from list, assigns order indices sequentially to list items.
Ordering(const std::list<Key> & L) ;
Ordering(const std::list<Key> & L);
/// Assignment operator
Ordering& operator=(const Ordering& rhs);
/// @}
/// @name Standard Interface
/// @{
/** One greater than the maximum ordering index, i.e. including missing indices in the count. See also size(). */
Index nVars() const { return nVars_; }
/** The actual number of variables in this ordering, i.e. not including missing indices in the count. See also nVars(). */
Index size() const { return order_.size(); }
Index size() const { return orderingIndex_.size(); }
const_iterator begin() const { return order_.begin(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */
const_iterator end() const { return order_.end(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */
Index at(Key key) const { return operator[](key); } ///< Synonym for operator[](Key) const
Key key(Index index) const {
if(index >= orderingIndex_.size())
throw std::out_of_range("Attempting to access out-of-range index in Ordering");
else
return orderingIndex_[index]->first; }
/** Assigns the ordering index of the requested \c key into \c index if the symbol
* is present in the ordering, otherwise does not modify \c index. The
@ -85,8 +94,7 @@ public:
index = i->second;
return true;
} else
return false;
}
return false; }
/// Access the index for the requested key, throws std::out_of_range if the
/// key is not present in the ordering (note that this differs from the
@ -122,23 +130,25 @@ public:
*/
const_iterator find(Key key) const { return order_.find(key); }
/**
* Attempts to insert a symbol/order pair with same semantics as stl::Map::insert(),
* i.e., returns a pair of iterator and success (false if already present)
*/
std::pair<iterator,bool> tryInsert(const value_type& key_order) {
std::pair<iterator,bool> it_ok(order_.insert(key_order));
if(it_ok.second == true && key_order.second+1 > nVars_)
nVars_ = key_order.second+1;
return it_ok;
}
/** Try insert, but will fail if the key is already present */
/** Insert a key-index pair, but will fail if the key is already present */
iterator insert(const value_type& key_order) {
std::pair<iterator,bool> it_ok(tryInsert(key_order));
if(!it_ok.second) throw std::invalid_argument(std::string("Attempting to insert a key into an ordering that already contains that key"));
else return it_ok.first;
}
std::pair<iterator,bool> it_ok(order_.insert(key_order));
if(it_ok.second) {
if(key_order.second >= orderingIndex_.size())
orderingIndex_.resize(key_order.second + 1);
orderingIndex_[key_order.second] = it_ok.first;
return it_ok.first;
} else
throw std::invalid_argument(std::string("Attempting to insert a key into an ordering that already contains that key")); }
/// Test if the key exists in the ordering.
bool exists(Key key) const { return order_.count(key) > 0; }
/** Insert a key-index pair, but will fail if the key is already present */
iterator insert(Key key, Index order) { return insert(std::make_pair(key,order)); }
/// Adds a new key to the ordering with an index of one greater than the current highest index.
Index push_back(Key key) { return insert(std::make_pair(key, orderingIndex_.size()))->second; }
/// @}
/// @name Advanced Interface
@ -154,18 +164,6 @@ public:
*/
iterator end() { return order_.end(); }
/// Test if the key exists in the ordering.
bool exists(Key key) const { return order_.count(key) > 0; }
///TODO: comment
std::pair<iterator,bool> tryInsert(Key key, Index order) { return tryInsert(std::make_pair(key,order)); }
///TODO: comment
iterator insert(Key key, Index order) { return insert(std::make_pair(key,order)); }
/// Adds a new key to the ordering with an index of one greater than the current highest index.
Index push_back(Key key) { return insert(std::make_pair(key, nVars_))->second; }
/** Remove the last (last-ordered, not highest-sorting key) symbol/index pair
* from the ordering (this version is \f$ O(n) \f$, use it when you do not
* know the last-ordered key).
@ -177,16 +175,6 @@ public:
*/
value_type pop_back();
/** Remove the last-ordered symbol from the ordering (this version is
* \f$ O(\log n) \f$, use it if you already know the last-ordered key).
*
* Throws std::invalid_argument if the requested key is not actually the
* last-ordered.
*
* @return The index of the symbol that was removed.
*/
Index pop_back(Key key);
/**
* += operator allows statements like 'ordering += x0,x1,x2,x3;', which are
* very useful for unit tests. This functionality is courtesy of
@ -201,17 +189,13 @@ public:
* internally, permuting an initial key-sorted ordering into a fill-reducing
* ordering.
*/
void permuteWithInverse(const Permutation& inversePermutation);
void permuteInPlace(const Permutation& permutation);
void permuteInPlace(const Permutation& selector, const Permutation& permutation);
/// Synonym for operator[](Key)
Index& at(Key key) { return operator[](key); }
/**
* Create an inverse mapping from Index->Key, useful for decoding linear systems
* @return inverse mapping structure
*/
InvertedMap invert() const;
/// @}
/// @name Testable
/// @{
@ -228,16 +212,26 @@ private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
template<class ARCHIVE>
void save(ARCHIVE & ar, const unsigned int version) const
{
ar & BOOST_SERIALIZATION_NVP(order_);
ar & BOOST_SERIALIZATION_NVP(nVars_);
}
size_t size_ = orderingIndex_.size(); // Save only the size but not the iterators
ar & BOOST_SERIALIZATION_NVP(size_);
}
template<class ARCHIVE>
void load(ARCHIVE & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(order_);
size_t size_;
ar & BOOST_SERIALIZATION_NVP(size_);
orderingIndex_.resize(size_);
for(iterator item = order_.begin(); item != order_.end(); ++item)
orderingIndex_[item->second] = item; // Assign the iterators
}
BOOST_SERIALIZATION_SPLIT_MEMBER()
}; // \class Ordering
// typedef for use with matlab
typedef Ordering::InvertedMap InvertedOrdering;
/**
* @class Unordered
* @brief a set of unordered indices
@ -263,14 +257,15 @@ public:
// Create an index formatter that looks up the Key in an inverse ordering, then
// formats the key using the provided key formatter, used in saveGraph.
struct OrderingIndexFormatter {
Ordering::InvertedMap inverseOrdering;
const KeyFormatter& keyFormatter;
class OrderingIndexFormatter {
private:
Ordering ordering_;
KeyFormatter keyFormatter_;
public:
OrderingIndexFormatter(const Ordering& ordering, const KeyFormatter& keyFormatter) :
inverseOrdering(ordering.invert()), keyFormatter(keyFormatter) {}
ordering_(ordering), keyFormatter_(keyFormatter) {}
std::string operator()(Index index) {
return keyFormatter(inverseOrdering.at(index));
}
return keyFormatter_(ordering_.key(index)); }
};
} // \namespace gtsam

2
gtsam/nonlinear/Values.cpp
View File

@ -81,7 +81,7 @@ namespace gtsam {
Values result;
for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
const SubVector& singleDelta = delta[ordering[key_value->key]]; // Delta for this value
const Vector& singleDelta = delta[ordering[key_value->key]]; // Delta for this value
Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument
Value* retractedValue(key_value->value.retract_(singleDelta)); // Retract
result.values_.insert(key, retractedValue); // Add retracted result directly to result values

63
gtsam/nonlinear/tests/testOrdering.cpp
View File

@ -23,26 +23,31 @@ using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST( testOrdering, simple_modifications ) {
TEST( Ordering, simple_modifications ) {
Ordering ordering;
// create an ordering
Symbol x1('x', 1), x2('x', 2), x3('x', 3), x4('x', 4);
ordering += x1, x2, x3, x4;
EXPECT_LONGS_EQUAL(0, ordering[x1]);
EXPECT_LONGS_EQUAL(1, ordering[x2]);
EXPECT_LONGS_EQUAL(2, ordering[x3]);
EXPECT_LONGS_EQUAL(3, ordering[x4]);
EXPECT_LONGS_EQUAL(Key(x1), ordering.key(0));
EXPECT_LONGS_EQUAL(Key(x2), ordering.key(1));
EXPECT_LONGS_EQUAL(Key(x3), ordering.key(2));
EXPECT_LONGS_EQUAL(Key(x4), ordering.key(3));
// pop the last two elements
Ordering::value_type x4p = ordering.pop_back();
EXPECT_LONGS_EQUAL(3, ordering.size());
EXPECT(assert_equal(x4, x4p.first));
Index x3p = ordering.pop_back(x3);
Index x3p = ordering.pop_back().second;
EXPECT_LONGS_EQUAL(2, ordering.size());
EXPECT_LONGS_EQUAL(2, (int)x3p);
// try to pop an element that doesn't exist and isn't last
CHECK_EXCEPTION(ordering.pop_back(x4), std::invalid_argument);
CHECK_EXCEPTION(ordering.pop_back(x1), std::invalid_argument);
// reassemble back make the ordering 1, 2, 4, 3
EXPECT_LONGS_EQUAL(2, ordering.push_back(x4));
EXPECT_LONGS_EQUAL(3, ordering.push_back(x3));
@ -50,6 +55,9 @@ TEST( testOrdering, simple_modifications ) {
EXPECT_LONGS_EQUAL(2, ordering[x4]);
EXPECT_LONGS_EQUAL(3, ordering[x3]);
EXPECT_LONGS_EQUAL(Key(x4), ordering.key(2));
EXPECT_LONGS_EQUAL(Key(x3), ordering.key(3));
// verify
Ordering expectedFinal;
expectedFinal += x1, x2, x4, x3;
@ -57,7 +65,36 @@ TEST( testOrdering, simple_modifications ) {
}
/* ************************************************************************* */
TEST( testOrdering, invert ) {
TEST(Ordering, permute) {
Ordering ordering;
ordering += 2, 4, 6, 8;
Ordering expected;
expected += 2, 8, 6, 4;
Permutation permutation(4);
permutation[0] = 0;
permutation[1] = 3;
permutation[2] = 2;
permutation[3] = 1;
Ordering actual = ordering;
actual.permuteInPlace(permutation);
EXPECT(assert_equal(expected, actual));
EXPECT_LONGS_EQUAL(0, actual[2]);
EXPECT_LONGS_EQUAL(1, actual[8]);
EXPECT_LONGS_EQUAL(2, actual[6]);
EXPECT_LONGS_EQUAL(3, actual[4]);
EXPECT_LONGS_EQUAL(2, actual.key(0));
EXPECT_LONGS_EQUAL(8, actual.key(1));
EXPECT_LONGS_EQUAL(6, actual.key(2));
EXPECT_LONGS_EQUAL(4, actual.key(3));
}
/* ************************************************************************* */
TEST( Ordering, invert ) {
// creates a map with the opposite mapping: Index->Key
Ordering ordering;
@ -65,14 +102,10 @@ TEST( testOrdering, invert ) {
Symbol x1('x', 1), x2('x', 2), x3('x', 3), x4('x', 4);
ordering += x1, x2, x3, x4;
Ordering::InvertedMap actual = ordering.invert();
Ordering::InvertedMap expected;
expected.insert(make_pair(0, x1));
expected.insert(make_pair(1, x2));
expected.insert(make_pair(2, x3));
expected.insert(make_pair(3, x4));
EXPECT(assert_container_equality(expected, actual));
EXPECT_LONGS_EQUAL(Key(x1), ordering.key(0));
EXPECT_LONGS_EQUAL(Key(x2), ordering.key(1));
EXPECT_LONGS_EQUAL(Key(x3), ordering.key(2));
EXPECT_LONGS_EQUAL(Key(x4), ordering.key(3));
}
/* ************************************************************************* */

35
tests/testDoglegOptimizer.cpp
View File

@ -32,6 +32,7 @@
#pragma GCC diagnostic pop
#include <boost/assign/list_of.hpp> // for 'list_of()'
#include <functional>
#include <boost/iterator/counting_iterator.hpp>
using namespace std;
using namespace gtsam;
@ -45,7 +46,8 @@ double computeError(const GaussianBayesNet& gbn, const LieVector& values) {
// Convert Vector to VectorValues
VectorValues vv = *allocateVectorValues(gbn);
vv.vector() = values;
internal::writeVectorValuesSlices(values, vv,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(vv.size()));
// Convert to factor graph
GaussianFactorGraph gfg(gbn);
@ -57,7 +59,8 @@ double computeErrorBt(const BayesTree<GaussianConditional>& gbt, const LieVector
// Convert Vector to VectorValues
VectorValues vv = *allocateVectorValues(gbt);
vv.vector() = values;
internal::writeVectorValuesSlices(values, vv,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(vv.size()));
// Convert to factor graph
GaussianFactorGraph gfg(gbt);
@ -89,20 +92,22 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPoint) {
// Compute the Hessian numerically
Matrix hessian = numericalHessian(
boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)),
LieVector(VectorValues::Zero(*allocateVectorValues(gbn)).vector()));
LieVector(VectorValues::Zero(*allocateVectorValues(gbn)).asVector()));
// Compute the gradient numerically
VectorValues gradientValues = *allocateVectorValues(gbn);
Vector gradient = numericalGradient(
boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)),
LieVector(VectorValues::Zero(gradientValues).vector()));
gradientValues.vector() = gradient;
LieVector(VectorValues::Zero(gradientValues).asVector()));
internal::writeVectorValuesSlices(gradient, gradientValues,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
// Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();
LONGS_EQUAL(11, augmentedHessian.cols());
VectorValues denseMatrixGradient = *allocateVectorValues(gbn);
denseMatrixGradient.vector() = -augmentedHessian.col(10).segment(0,10);
internal::writeVectorValuesSlices(-augmentedHessian.col(10).segment(0,10), denseMatrixGradient,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5));
// Compute the steepest descent point
@ -269,20 +274,22 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) {
// Compute the Hessian numerically
Matrix hessian = numericalHessian(
boost::function<double(const LieVector&)>(boost::bind(&computeErrorBt, bt, _1)),
LieVector(VectorValues::Zero(*allocateVectorValues(bt)).vector()));
LieVector(VectorValues::Zero(*allocateVectorValues(bt)).asVector()));
// Compute the gradient numerically
VectorValues gradientValues = *allocateVectorValues(bt);
Vector gradient = numericalGradient(
boost::function<double(const LieVector&)>(boost::bind(&computeErrorBt, bt, _1)),
LieVector(VectorValues::Zero(gradientValues).vector()));
gradientValues.vector() = gradient;
LieVector(VectorValues::Zero(gradientValues).asVector()));
internal::writeVectorValuesSlices(gradient, gradientValues,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
// Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian();
LONGS_EQUAL(11, augmentedHessian.cols());
VectorValues denseMatrixGradient = *allocateVectorValues(bt);
denseMatrixGradient.vector() = -augmentedHessian.col(10).segment(0,10);
internal::writeVectorValuesSlices(-augmentedHessian.col(10).segment(0,10), denseMatrixGradient,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5));
// Compute the steepest descent point
@ -338,12 +345,12 @@ TEST(DoglegOptimizer, ComputeBlend) {
VectorValues xn = optimize(gbn);
// The Newton's method point should be more "adventurous", i.e. larger, than the steepest descent point
EXPECT(xu.vector().norm() < xn.vector().norm());
EXPECT(xu.asVector().norm() < xn.asVector().norm());
// Compute blend
double Delta = 1.5;
VectorValues xb = DoglegOptimizerImpl::ComputeBlend(Delta, xu, xn);
DOUBLES_EQUAL(Delta, xb.vector().norm(), 1e-10);
DOUBLES_EQUAL(Delta, xb.asVector().norm(), 1e-10);
}
/* ************************************************************************* */
@ -371,12 +378,12 @@ TEST(DoglegOptimizer, ComputeDoglegPoint) {
double Delta1 = 0.5; // Less than steepest descent
VectorValues actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, optimizeGradientSearch(gbn), optimize(gbn));
DOUBLES_EQUAL(Delta1, actual1.vector().norm(), 1e-5);
DOUBLES_EQUAL(Delta1, actual1.asVector().norm(), 1e-5);
double Delta2 = 1.5; // Between steepest descent and Newton's method
VectorValues expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, optimizeGradientSearch(gbn), optimize(gbn));
VectorValues actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, optimizeGradientSearch(gbn), optimize(gbn));
DOUBLES_EQUAL(Delta2, actual2.vector().norm(), 1e-5);
DOUBLES_EQUAL(Delta2, actual2.asVector().norm(), 1e-5);
EXPECT(assert_equal(expected2, actual2));
double Delta3 = 5.0; // Larger than Newton's method point

2
tests/testGaussianFactorGraphB.cpp
View File

@ -252,7 +252,7 @@ TEST( GaussianFactorGraph, getOrdering)
Ordering original; original += L(1),X(1),X(2);
FactorGraph<IndexFactor> symbolic(createGaussianFactorGraph(original));
Permutation perm(*inference::PermutationCOLAMD(VariableIndex(symbolic)));
Ordering actual = original; actual.permuteWithInverse((*perm.inverse()));
Ordering actual = original; actual.permuteInPlace(perm);
Ordering expected; expected += L(1),X(2),X(1);
EXPECT(assert_equal(expected,actual));
}

1
tests/testGaussianISAM2.cpp
View File

@ -207,6 +207,7 @@ TEST_UNSAFE(ISAM2, ImplAddVariables) {
EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys));
EXPECT(assert_equal(orderingExpected, ordering));
}
/* ************************************************************************* */
TEST_UNSAFE(ISAM2, ImplRemoveVariables) {

100
tests/timeiSAM2Chain.cpp Normal file
View File

@ -0,0 +1,100 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file timeiSAM2Chain.cpp
* @brief Times each iteration of a long chain in iSAM2, to measure asymptotic performance
* @author Richard Roberts
*/
#include <gtsam/base/timing.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/Marginals.h>
#include <fstream>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/export.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
using namespace std;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
typedef Pose2 Pose;
typedef NoiseModelFactor1<Pose> NM1;
typedef NoiseModelFactor2<Pose,Pose> NM2;
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(Pose::Dim());
int main(int argc, char *argv[]) {
const size_t steps = 50000;
cout << "Playing forward " << steps << " time steps..." << endl;
ISAM2 isam2;
// Times
vector<double> times(steps);
for(size_t step=0; step < steps; ++step) {
Values newVariables;
NonlinearFactorGraph newFactors;
// Collect measurements and new variables for the current step
gttic_(Create_measurements);
if(step == 0) {
// Add prior
newFactors.add(PriorFactor<Pose>(0, Pose(), noiseModel::Unit::Create(Pose::Dim())));
newVariables.insert(0, Pose());
} else {
Vector eta = Vector::Random(Pose::Dim()) * 0.1;
Pose2 between = Pose().retract(eta);
// Add between
newFactors.add(BetweenFactor<Pose>(step-1, step, between, model));
newVariables.insert(step, isam2.calculateEstimate<Pose>(step-1) * between);
}
gttoc_(Create_measurements);
// Update iSAM2
gttic_(Update_ISAM2);
isam2.update(newFactors, newVariables);
gttoc_(Update_ISAM2);
tictoc_finishedIteration_();
tictoc_getNode(node, Update_ISAM2);
times[step] = node->time();
if(step % 1000 == 0) {
cout << "Step " << step << endl;
tictoc_print_();
}
}
tictoc_print_();
// Write times
ofstream timesFile("times.txt");
BOOST_FOREACH(double t, times) {
timesFile << t << "\n"; }
return 0;
}