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

Working on unordered VectorValues.

release/4.3a0
Richard Roberts 2013-07-04 03:20:46 +00:00
parent 6a6e7d012b
commit 530bde404e
2 changed files with 126 additions and 313 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

175
gtsam/linear/VectorValuesUnordered.cpp
View File

@ -16,51 +16,22 @@
* @author Alex Cunningham
*/
#include <gtsam/base/FastVector.h>
#include <gtsam/linear/VectorValuesUnordered.h>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/range/combine.hpp>
#include <boost/range/numeric.hpp>
#include <boost/range/adaptor/map.hpp>
#include <boost/range/adaptor/transformed.hpp>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
VectorValuesUnordered VectorValuesUnordered::Zero(const VectorValuesUnordered& x) {
VectorValuesUnordered 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;
}
/* ************************************************************************* */
vector<size_t> VectorValuesUnordered::dims() const {
std::vector<size_t> result(this->size());
for(Index j = 0; j < this->size(); ++j)
result[j] = this->dim(j);
return result;
}
/* ************************************************************************* */
void VectorValuesUnordered::insert(Index j, const Vector& value) {
// Make sure j does not already exist
if(exists(j))
throw invalid_argument("VectorValues: requested variable index to insert already exists.");
// If this adds variables at the end, insert zero-length entries up to j
if(j >= size())
values_.resize(j+1);
// Assign value
values_[j] = value;
}
/* ************************************************************************* */
void VectorValuesUnordered::print(const std::string& str, const IndexFormatter& formatter) const {
void VectorValuesUnordered::print(const std::string& str, const KeyFormatter& formatter) const {
std::cout << str << ": " << size() << " elements\n";
for (Index var = 0; var < size(); ++var)
std::cout << " " << formatter(var) << ": \n" << (*this)[var] << "\n";
BOOST_FOREACH(const value_type& key_value, *this)
std::cout << " " << formatter(key_value.first) << ": \n" << key_value.second.transpose() << "\n";
std::cout.flush();
}
@ -68,85 +39,77 @@ void VectorValuesUnordered::print(const std::string& str, const IndexFormatter&
bool VectorValuesUnordered::equals(const VectorValuesUnordered& x, double tol) const {
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))
typedef boost::tuple<value_type, value_type> ValuePair;
BOOST_FOREACH(const ValuePair& values, boost::combine(*this, x)) {
if(values.get<0>.first != values.get<1>.first ||
!equal_with_abs_tol(values.get<0>.second, values.get<1>.second, tol))
return false;
return true;
}
/* ************************************************************************* */
void VectorValuesUnordered::resize(Index nVars, size_t varDim) {
values_.resize(nVars);
for(Index j = 0; j < nVars; ++j)
values_[j] = Vector(varDim);
}
/* ************************************************************************* */
void VectorValuesUnordered::resizeLike(const VectorValuesUnordered& other) {
values_.resize(other.size());
for(Index j = 0; j < other.size(); ++j)
values_[j].resize(other.values_[j].size());
}
/* ************************************************************************* */
VectorValuesUnordered VectorValuesUnordered::SameStructure(const VectorValuesUnordered& other) {
VectorValuesUnordered ret;
ret.resizeLike(other);
return ret;
}
/* ************************************************************************* */
VectorValuesUnordered VectorValuesUnordered::Zero(Index nVars, size_t varDim) {
VectorValuesUnordered ret(nVars, varDim);
ret.setZero();
return ret;
}
/* ************************************************************************* */
void VectorValuesUnordered::setZero() {
BOOST_FOREACH(Vector& v, *this) {
v.setZero();
}
}
/* ************************************************************************* */
const Vector VectorValuesUnordered::asVector() const {
return internal::extractVectorValuesSlices(*this,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(this->size()), true);
}
/* ************************************************************************* */
const Vector VectorValuesUnordered::vector(const std::vector<Index>& indices) const {
return internal::extractVectorValuesSlices(*this, indices.begin(), indices.end());
}
/* ************************************************************************* */
bool VectorValuesUnordered::hasSameStructure(const VectorValuesUnordered& other) const {
if(this->size() != other.size())
return false;
for(size_t j = 0; j < size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
if(this->values_[j].rows() != other.values_[j].rows())
return false;
return true;
}
/* ************************************************************************* */
const Vector VectorValuesUnordered::asVector() const
{
using boost::adaptors::map_values;
using boost::adaptors::transformed;
// Count dimensions
const DenseIndex totalDim = boost::accumulate(*this | map_values | transformed(&Vector::size), 0);
// Copy vectors
Vector result;
DenseIndex pos = 0;
BOOST_FOREACH(const Vector& v, *this | map_values) {
result.segment(pos, v.size()) = v;
pos += v.size();
}
return result;
}
/* ************************************************************************* */
const Vector VectorValuesUnordered::vector(const std::vector<Key>& keys) const
{
// Count dimensions and collect pointers to avoid double lookups
DenseIndex totalDim = 0;
std::vector<const Vector*> items(keys.size());
for(size_t i = 0; i < keys.size(); ++i) {
items[i] = &at(i);
totalDim += items[i]->size();
}
// Copy vectors
Vector result(totalDim);
DenseIndex pos = 0;
BOOST_FOREACH(const Vector *v, items) {
result.segment(pos, v->size()) = *v;
pos += v->size();
}
return result;
}
/* ************************************************************************* */
void VectorValuesUnordered::swap(VectorValuesUnordered& other) {
this->values_.swap(other.values_);
}
/* ************************************************************************* */
double VectorValuesUnordered::dot(const VectorValuesUnordered& v) const {
double result = 0.0;
double VectorValuesUnordered::dot(const VectorValuesUnordered& v) const
{
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");
throw invalid_argument("VectorValues::dot called with a VectorValues of different structure");
double result = 0.0;
typedef boost::tuple<value_type, value_type> ValuePair;
using boost::adaptors::map_values;
BOOST_FOREACH(const ValuePair& values, boost::combine(*this, v)) {
assert_throw(values.get<0>().first == values.get<1>.first,
std::invalid_argument("VectorValues::dot called with a VectorValues of different structure"));
assert_throw(values.get<0>().second.size() == values.get<1>().second.size(),
std::invalid_argument("VectorValues::dot called with a VectorValues of different structure"));
result += values.get<0>().second.dot(values.get<1>().second);
}
return result;
}
@ -158,9 +121,9 @@ double VectorValuesUnordered::norm() const {
/* ************************************************************************* */
double VectorValuesUnordered::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();
using boost::adaptors::map_values;
BOOST_FOREACH(const Vector& v, *this | map_values)
sumSquares += v.squaredNorm();
return sumSquares;
}

264
gtsam/linear/VectorValuesUnordered.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/base/Vector.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/global_includes.h>
#include <boost/format.hpp>
@ -39,9 +40,9 @@ namespace gtsam {
* or creating this class in unit tests and examples where speed is not important,
* you can use a simple interface:
* - The default constructor VectorValues() to create this class
* - insert(Index, const Vector&) to add vector variables
* - operator[](Index) for read and write access to stored variables
* - \ref exists (Index) to check if a variable is present
* - insert(Key, const Vector&) to add vector variables
* - operator[](Key) for read and write access to stored variables
* - \ref exists (Key) to check if a variable is present
* - Other facilities like iterators, size(), dim(), etc.
*
* Indices can be non-consecutive and inserted out-of-order, but you should not
@ -75,7 +76,7 @@ namespace gtsam {
* - Allocate space ahead of time using a pre-allocating constructor
* (\ref AdvancedConstructors "Advanced Constructors"), Zero(),
* SameStructure(), resize(), or append(). Do not use
* insert(Index, const Vector&), which always has to re-allocate the
* insert(Key, const Vector&), which always has to re-allocate the
* internal vector.
* - The vector() function permits access to the underlying Vector, for
* doing mathematical or other operations that require all values.
@ -90,7 +91,7 @@ namespace gtsam {
*/
class GTSAM_EXPORT VectorValuesUnordered {
protected:
typedef std::vector<Vector> Values; ///< Typedef for the collection of Vectors making up a VectorValues
typedef FastMap<Key, Vector> Values; ///< Typedef for the collection of Vectors making up a VectorValues
Values values_; ///< Collection of Vectors making up this VectorValues
public:
@ -99,6 +100,7 @@ namespace gtsam {
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<VectorValuesUnordered> shared_ptr; ///< shared_ptr to this class
typedef Values::value_type value_type; ///< Typedef to pair<Key, Vector>, a key-value pair
/// @name Standard Constructors
/// @{
@ -108,48 +110,54 @@ namespace gtsam {
*/
VectorValuesUnordered() {}
/** Named constructor to create a VectorValues of the same structure of the
* specified one, but filled with zeros.
* @return
*/
static VectorValuesUnordered Zero(const VectorValuesUnordered& model);
/// @}
/// @name Standard Interface
/// @{
/** 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 values_.size(); }
/** Number of variables stored. */
Key size() const { return values_.size(); }
/** Return the dimension of variable \c j. */
size_t dim(Index j) const { checkExists(j); return (*this)[j].rows(); }
size_t dim(Key j) const { return at(j).rows(); }
/** Return the dimension of each vector in this container */
std::vector<size_t> dims() const;
/** Check whether a variable with key \c j exists. */
bool exists(Key j) const { return find(j) != end(); }
/** Check whether a variable with index \c j exists. */
bool exists(Index j) const { return j < size() && values_[j].rows() > 0; }
/** Read/write access to the vector value with key \c j, throws std::out_of_range if \c j does not exist, identical to operator[](Key). */
Vector& at(Key j) {
iterator item = find(j);
if(item == end())
throw std::out_of_range(
"Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues.");
else
return item->second;
}
/** 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). */
Vector& at(Index j) { checkExists(j); return values_[j]; }
/** Access the vector value with key \c j (const version), throws std::out_of_range if \c j does not exist, identical to operator[](Key). */
const Vector& at(Key j) const {
const_iterator item = find(j);
if(item == end())
throw std::out_of_range(
"Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues.");
else
return item->second;
}
/** 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 Vector& at(Index j) const { checkExists(j); return values_[j]; }
/** Read/write access to the vector value with key \c j, throws std::out_of_range if \c j does not exist, identical to at(Key). */
Vector& operator[](Key j) { return at(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). */
Vector& operator[](Index j) { return at(j); }
/** Access the vector value with key \c j (const version), throws std::out_of_range if \c j does not exist, identical to at(Key). */
const Vector& operator[](Key j) const { 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 Vector& operator[](Index j) const { return at(j); }
/** Insert a vector \c value with index \c j.
* Causes reallocation, but can insert values in any order.
* Throws an invalid_argument exception if the index \c j is already used.
/** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c j is already used.
* @param value The vector to be inserted.
* @param j The index with which the value will be associated.
*/
void insert(Index j, const Vector& value);
void insert(Key j, const Vector& value) {
if(!values_.insert(std::make_pair(j, value)).second)
throw std::invalid_argument(
"Requested to insert variable '" + DefaultKeyFormatter(j) + "' already in this VectorValues.");
}
iterator begin() { return values_.begin(); } ///< Iterator over variables
const_iterator begin() const { return values_.begin(); } ///< Iterator over variables
@ -160,99 +168,28 @@ namespace gtsam {
reverse_iterator rend() { return values_.rend(); } ///< Reverse iterator over variables
const_reverse_iterator rend() const { return values_.rend(); } ///< Reverse iterator over variables
/** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */
iterator find(Key j) { return values_.find(j); }
/** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */
const_iterator find(Key j) const { return values_.find(j); }
/** print required by Testable for unit testing */
void print(const std::string& str = "VectorValues: ",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
const KeyFormatter& formatter = DefaultKeyFormatter) const;
/** equals required by Testable for unit testing */
bool equals(const VectorValuesUnordered& x, double tol = 1e-9) const;
/// @{
/// \anchor AdvancedConstructors
/// @name Advanced Constructors
/// @}
/** Construct from a container of variable dimensions (in variable order), without initializing any values. */
template<class CONTAINER>
explicit VectorValuesUnordered(const CONTAINER& dimensions) { this->append(dimensions); }
/** Construct to hold nVars vectors of varDim dimension each. */
VectorValuesUnordered(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. */
static VectorValuesUnordered SameStructure(const VectorValuesUnordered& other);
/** Named constructor to create a VectorValues from a container of variable
* dimensions that is filled with zeros.
* @param dimensions A container of the dimension of each variable to create.
*/
template<class CONTAINER>
static VectorValuesUnordered Zero(const CONTAINER& dimensions);
/** Named constructor to create a VectorValues filled with zeros that has
* \c nVars variables, each of dimension \c varDim
* @param nVars The number of variables to create
* @param varDim The dimension of each variable
* @return The new VectorValues
*/
static VectorValuesUnordered Zero(Index nVars, size_t varDim);
/// @}
/// @name Advanced Interface
/// @{
/** Resize this VectorValues to have identical structure to other, leaving
* this VectorValues with uninitialized values.
* @param other The VectorValues whose structure to copy
*/
void resizeLike(const VectorValuesUnordered& other);
/** Resize the VectorValues to hold \c nVars variables, each of dimension
* \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. 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>
void resize(const CONTAINER& dimensions);
/** Append to the VectorValues to additionally contain variables of the
* dimensions stored in \c dimensions. The new variables are uninitialized,
* but this function is used to pre-allocate space for performance. This
* function preserves the original data, so all previously-existing variables
* are left unchanged.
* @param dimensions A container of the dimension of each variable to create.
*/
template<class CONTAINER>
void append(const CONTAINER& dimensions);
/** Removes the last subvector from the VectorValues */
void pop_back() { values_.pop_back(); };
/** Set all entries to zero, does not modify the size. */
void setZero();
/** Retrieve the entire solution as a single vector */
const Vector asVector() const;
/** Access a vector that is a subset of relevant indices */
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,
* as another VectorValues
* @param other The other VectorValues with which to compare structure
* @return \c true if the structure is the same, \c false if not.
*/
bool hasSameStructure(const VectorValuesUnordered& other) const;
/** Access a vector that is a subset of relevant keys. */
const Vector vector(const std::vector<Key>& keys) const;
/**
* Swap the data in this VectorValues with another.
@ -264,7 +201,8 @@ namespace gtsam {
/// @{
/** Dot product with another VectorValues, interpreting both as vectors of
* their concatenated values. */
* their concatenated values. Both VectorValues must have the
* same structure (checked when NDEBUG is not defined). */
double dot(const VectorValuesUnordered& v) const;
/** Vector L2 norm */
@ -302,38 +240,26 @@ namespace gtsam {
/// @}
private:
// Throw an exception if j does not exist
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();
throw std::out_of_range(msg);
}
}
public:
/**
* scale a vector by a scalar
*/
friend VectorValuesUnordered operator*(const double a, const VectorValuesUnordered &v) {
VectorValuesUnordered result = VectorValuesUnordered::SameStructure(v);
for(Index j = 0; j < v.size(); ++j)
for(Key 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 void scal(double alpha, VectorValuesUnordered& x) {
for(Index j = 0; j < x.size(); ++j)
for(Key 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 VectorValuesUnordered& x, VectorValuesUnordered& y) {
if(x.size() != y.size())
throw std::invalid_argument("axpy(VectorValues) called with different vector sizes");
for(Index j = 0; j < x.size(); ++j)
for(Key j = 0; j < x.size(); ++j)
if(x.values_[j].size() == y.values_[j].size())
y.values_[j] += alpha * x.values_[j];
else
@ -341,7 +267,7 @@ namespace gtsam {
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend void sqrt(VectorValuesUnordered &x) {
for(Index j = 0; j < x.size(); ++j)
for(Key j = 0; j < x.size(); ++j)
x.values_[j] = x.values_[j].cwiseSqrt();
}
@ -349,7 +275,7 @@ namespace gtsam {
friend void ediv(const VectorValuesUnordered& numerator, const VectorValuesUnordered& denominator, VectorValuesUnordered &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)
for(Key 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
@ -360,7 +286,7 @@ namespace gtsam {
friend void edivInPlace(VectorValuesUnordered& x, const VectorValuesUnordered& y) {
if(x.size() != y.size())
throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes");
for(Index j = 0; j < x.size(); ++j)
for(Key j = 0; j < x.size(); ++j)
if(x.values_[j].size() == y.values_[j].size())
x.values_[j].array() /= y.values_[j].array();
else
@ -376,80 +302,4 @@ namespace gtsam {
}
}; // VectorValues definition
// Implementations of template and inline functions
/* ************************************************************************* */
template<class CONTAINER>
void VectorValuesUnordered::resize(const CONTAINER& dimensions) {
values_.clear();
append(dimensions);
}
/* ************************************************************************* */
template<class CONTAINER>
void VectorValuesUnordered::append(const CONTAINER& dimensions) {
size_t i = size();
values_.resize(size() + dimensions.size());
BOOST_FOREACH(size_t dim, dimensions) {
values_[i] = Vector(dim);
++ i;
}
}
/* ************************************************************************* */
template<class CONTAINER>
VectorValuesUnordered VectorValuesUnordered::Zero(const CONTAINER& dimensions) {
VectorValuesUnordered ret;
ret.values_.resize(dimensions.size());
size_t i = 0;
BOOST_FOREACH(size_t dim, dimensions) {
ret.values_[i] = Vector::Zero(dim);
++ i;
}
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<typename ITERATOR>
const Vector extractVectorValuesSlices(const VectorValuesUnordered& values, ITERATOR first, ITERATOR last, bool allowNonexistant = false) {
// Find total dimensionality
size_t 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);
size_t 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;
}
/* ************************************************************************* */
// 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, VectorValuesUnordered& values, ITERATOR first, ITERATOR last) {
// Copy vectors
size_t 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());
}
}
} // \namespace gtsam