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

Simplified VectorValues interface and implementation, slight simplification to Permuted<>

release/4.3a0
Richard Roberts 2011-10-26 02:04:06 +00:00
parent 48d9c82713
commit 2c53df3ee7
19 changed files with 765 additions and 611 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/inference/Permutation.h
View File

@ -151,12 +151,12 @@ protected:
* container[permutation[index2]];
* but more concise.
*/
template<typename CONTAINER, typename VALUETYPE = typename CONTAINER::value_reference_type>
template<typename CONTAINER>
class Permuted {
Permutation permutation_;
CONTAINER& container_;
public:
typedef VALUETYPE value_type;
typedef typename CONTAINER::iterator::value_type value_type;
/** Construct as a permuted view on the Container. The permutation is copied
* but only a reference to the container is stored.
@ -169,10 +169,10 @@ public:
Permuted(CONTAINER& container) : permutation_(Permutation::Identity(container.size())), container_(container) {}
/** Access the container through the permutation */
value_type operator[](size_t index) const { return container_[permutation_[index]]; }
value_type& operator[](size_t index) { return container_[permutation_[index]]; }
// /** Access the container through the permutation (const version) */
// const_value_type operator[](size_t index) const;
/** Access the container through the permutation (const version) */
const value_type& operator[](size_t index) const { return container_[permutation_[index]]; }
/** Permute this view by applying a permutation to the underlying permutation */
void permute(const Permutation& permutation) { assert(permutation.size() == this->size()); permutation_ = *permutation_.permute(permutation); }

2
gtsam/inference/VariableIndex.h
View File

@ -45,7 +45,7 @@ public:
protected:
std::vector<Factors> indexUnpermuted_;
Permuted<std::vector<Factors>, Factors&> index_; // Permuted view of indexUnpermuted.
Permuted<std::vector<Factors> > index_; // Permuted view of indexUnpermuted.
size_t nFactors_; // Number of factors in the original factor graph.
size_t nEntries_; // Sum of involved variable counts of each factor.

53
gtsam/linear/GaussianConditional.cpp
View File

@ -27,6 +27,42 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
// Helper function used only in this file - extracts vectors with variable indices
// in the first and last iterators, and concatenates them in that order into the
// output.
template<typename ITERATOR>
static Vector extractVectorValuesSlices(const VectorValues& values, ITERATOR first, ITERATOR last) {
// Find total dimensionality
int dim = 0;
for(ITERATOR j = first; j != last; ++j)
dim += values.dim(*j);
// Copy vectors
Vector ret(dim);
int varStart = 0;
for(ITERATOR j = first; j != last; ++j) {
ret.segment(varStart, values.dim(*j)) = values[*j];
varStart += values.dim(*j);
}
return ret;
}
/* ************************************************************************* */
// Helper function used only in this file - 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>
static 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.dim(*j));
varStart += values.dim(*j);
}
assert(varStart = vector.rows());
}
/* ************************************************************************* */
GaussianConditional::GaussianConditional() : rsd_(matrix_) {}
@ -169,14 +205,13 @@ JacobianFactor::shared_ptr GaussianConditional::toFactor() const {
/* ************************************************************************* */
void GaussianConditional::rhs(VectorValues& x) const {
Vector d = rhs();
x.range(beginFrontals(), endFrontals(), d);
writeVectorValuesSlices(get_d(), x, beginFrontals(), endFrontals());
}
/* ************************************************************************* */
void GaussianConditional::rhs(Permuted<VectorValues>& x) const {
// Copy the rhs into x, accounting for the permutation
Vector d = rhs();
Vector d = get_d();
size_t rhsPosition = 0; // We walk through the rhs by variable
for(const_iterator j = beginFrontals(); j != endFrontals(); ++j) {
// Get the segment of the rhs for this variable
@ -190,7 +225,7 @@ void GaussianConditional::rhs(Permuted<VectorValues>& x) const {
void GaussianConditional::solveInPlace(VectorValues& x) const {
static const bool debug = false;
if(debug) print("Solving conditional in place");
Vector rhs = x.range(beginFrontals(), endFrontals());
Vector rhs = extractVectorValuesSlices(x, beginFrontals(), endFrontals());
for (const_iterator parent = beginParents(); parent != endParents(); ++parent) {
rhs += -get_S(parent) * x[*parent];
}
@ -200,7 +235,7 @@ void GaussianConditional::solveInPlace(VectorValues& x) const {
gtsam::print(rhs, "rhs: ");
gtsam::print(soln, "full back-substitution solution: ");
}
x.range(beginFrontals(), endFrontals(), soln);
writeVectorValuesSlices(soln, x, beginFrontals(), endFrontals());
}
/* ************************************************************************* */
@ -245,7 +280,7 @@ VectorValues GaussianConditional::solve(const VectorValues& x) const {
/* ************************************************************************* */
void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
Vector frontalVec = gy.range(beginFrontals(), endFrontals());
Vector frontalVec = extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
// TODO: verify permutation
frontalVec = permutation_ * gtsam::backSubstituteUpper(frontalVec,Matrix(get_R()));
GaussianConditional::const_iterator it;
@ -253,14 +288,14 @@ void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
const Index i = *it;
transposeMultiplyAdd(-1.0,get_S(it),frontalVec,gy[i]);
}
gy.range(beginFrontals(), endFrontals(), frontalVec);
writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
}
/* ************************************************************************* */
void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const {
Vector frontalVec = gy.range(beginFrontals(), endFrontals());
Vector frontalVec = extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
frontalVec = emul(frontalVec, get_sigmas());
gy.range(beginFrontals(), endFrontals(), frontalVec);
writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
}
}

28
gtsam/linear/IterativeOptimizationParameters.h
View File

@ -12,6 +12,8 @@
namespace gtsam {
struct DimSpec;
// a container for all related parameters
struct IterativeOptimizationParameters {
@ -31,7 +33,7 @@ public:
double epsilon_abs_; // absolute error
verbosityLevel verbosity_;
size_t nReduce_ ;
DimSpec::shared_ptr skeleton_spec_;
boost::shared_ptr<DimSpec> skeleton_spec_;
bool est_cond_ ;
public:
@ -80,4 +82,28 @@ public:
return est_cond_ ;
}
};
struct DimSpec: public std::vector<size_t> {
typedef std::vector<size_t> Base;
typedef boost::shared_ptr<DimSpec> shared_ptr;
DimSpec() :
Base() {
}
DimSpec(size_t n) :
Base(n) {
}
DimSpec(size_t n, size_t init) :
Base(n, init) {
}
DimSpec(const VectorValues &V) :
Base(V.size()) {
const size_t n = V.size();
for (size_t i = 0; i < n; ++i) {
(*this)[i] = V[i].rows();
}
}
};
}

6
gtsam/linear/JacobianFactor.cpp
View File

@ -580,7 +580,7 @@ namespace gtsam {
/* ************************************************************************* */
VectorValues gradient(const FactorGraph<JacobianFactor>& fg, const VectorValues& x) {
// It is crucial for performance to make a zero-valued clone of x
VectorValues g = VectorValues::zero(x);
VectorValues g = VectorValues::Zero(x);
Errors e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
e.push_back(factor->error_vector(x));
@ -603,7 +603,7 @@ namespace gtsam {
/* ************************************************************************* */
void multiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r) {
r.makeZero();
r.vector() = Vector::Zero(r.dim());
Index i = 0;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
@ -615,7 +615,7 @@ namespace gtsam {
/* ************************************************************************* */
void transposeMultiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &r, VectorValues &x) {
x.makeZero();
x.vector() = Vector::Zero(x.dim());
Index i = 0;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {

4
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -64,7 +64,7 @@ namespace gtsam {
VectorValues gradient(const SubgraphPreconditioner& sp, const VectorValues& y) {
VectorValues x = sp.x(y); // x = inv(R1)*y
Errors e2 = gaussianErrors(*sp.Ab2(),x);
VectorValues gx2 = VectorValues::zero(y);
VectorValues gx2 = VectorValues::Zero(y);
gtsam::transposeMultiplyAdd(*sp.Ab2(),1.0,e2,gx2); // A2'*e2;
VectorValues gy2 = gtsam::backSubstituteTranspose(*sp.Rc1(), gx2); // inv(R1')*gx2
return y + gy2;
@ -138,7 +138,7 @@ namespace gtsam {
while (it != end)
e2.push_back(*(it++));
VectorValues x = VectorValues::zero(y); // x = 0
VectorValues x = VectorValues::Zero(y); // x = 0
gtsam::transposeMultiplyAdd(*Ab2_,1.0,e2,x); // x += A2'*e2
axpy(alpha, gtsam::backSubstituteTranspose(*Rc1_, x), y); // y += alpha*inv(R1')*x
}

3
gtsam/linear/SubgraphPreconditioner.h
View File

@ -76,8 +76,7 @@ namespace gtsam {
/* A zero VectorValues with the structure of xbar */
VectorValues zero() const {
VectorValues V(*xbar_) ;
V.makeZero();
VectorValues V(VectorValues::Zero(*xbar_)) ;
return V ;
}

145
gtsam/linear/VectorValues.cpp
View File

@ -16,56 +16,69 @@
* @author Alex Cunningham
*/
#include <gtsam/base/FastVector.h>
#include <gtsam/linear/VectorValues.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
VectorValues::VectorValues(Index nVars, size_t varDim) :
varStarts_(nVars + 1) {
varStarts_[0] = 0;
size_t varStart = 0;
for (Index j = 1; j <= nVars; ++j)
varStarts_[j] = (varStart += varDim);
values_.resize(varStarts_.back());
VectorValues::VectorValues(const VectorValues& other) {
*this = other;
}
/* ************************************************************************* */
VectorValues::VectorValues(const std::vector<size_t>& dimensions,
const Vector& values) :
values_(values), varStarts_(dimensions.size() + 1) {
varStarts_[0] = 0;
size_t varStart = 0;
Index var = 0;
BOOST_FOREACH(size_t dim, dimensions)
varStarts_[++var] = (varStart += dim);
assert(varStarts_.back() == (size_t) values.size());
VectorValues& VectorValues::operator=(const VectorValues& rhs) {
if(this != &rhs) {
resizeLike(rhs); // Copy structure
values_ = rhs.values_; // Copy values
}
return *this;
}
/* ************************************************************************* */
VectorValues::VectorValues(const std::vector<size_t>& dimensions,
const double* values) :
varStarts_(dimensions.size() + 1) {
varStarts_[0] = 0;
size_t varStart = 0;
Index var = 0;
BOOST_FOREACH(size_t dim, dimensions)
varStarts_[++var] = (varStart += dim);
values_ = Vector_(varStart, values);
VectorValues VectorValues::Zero(const VectorValues& x) {
VectorValues cloned(SameStructure(x));
cloned.vector() = Vector::Zero(x.dim());
return cloned;
}
/* ************************************************************************* */
VectorValues VectorValues::SameStructure(const VectorValues& otherValues) {
VectorValues ret;
ret.varStarts_ = otherValues.varStarts_;
ret.values_.resize(ret.varStarts_.back());
return ret;
void VectorValues::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.");
// 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);
// 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;
}
/* ************************************************************************* */
void VectorValues::print(const std::string& str) const {
std::cout << str << ": " << varStarts_.size() - 1 << " elements\n";
std::cout << str << ": " << size() << " elements\n";
for (Index var = 0; var < size(); ++var)
std::cout << " " << var << ": \n" << operator[](var) << "\n";
std::cout.flush();
@ -73,61 +86,59 @@ void VectorValues::print(const std::string& str) const {
/* ************************************************************************* */
bool VectorValues::equals(const VectorValues& x, double tol) const {
return varStarts_ == x.varStarts_
&& equal_with_abs_tol(values_, x.values_, tol);
return hasSameStructure(x) && equal_with_abs_tol(values_, x.values_, tol);
}
/* ************************************************************************* */
size_t VectorValues::dim(Index j) const {
checkVariable(j);
const size_t start = varStarts_[j], n = varStarts_[j + 1] - start;
return n;
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;
}
}
/* ************************************************************************* */
VectorValues::mapped_type VectorValues::operator[](Index j) {
checkVariable(j);
const size_t start = varStarts_[j], n = varStarts_[j + 1] - start;
return values_.segment(start, n);
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();
}
}
/* ************************************************************************* */
VectorValues::const_mapped_type VectorValues::operator[](Index j) const {
checkVariable(j);
const size_t start = varStarts_[j], n = varStarts_[j + 1] - start;
return values_.segment(start, n);
VectorValues VectorValues::SameStructure(const VectorValues& other) {
VectorValues ret;
ret.resizeLike(other);
return ret;
}
/* ************************************************************************* */
VectorValues VectorValues::zero(const VectorValues& x) {
VectorValues cloned(x);
cloned.makeZero();
return cloned;
}
/* ************************************************************************* */
Index VectorValues::push_back_preallocated(const Vector& vector) {
Index var = varStarts_.size() - 1;
varStarts_.push_back(varStarts_.back() + vector.size());
this->operator[](var) = vector; // This will assert that values_ has enough allocated space.
return var;
bool VectorValues::hasSameStructure(const VectorValues& other) const {
for(size_t j=0; j<size(); ++j)
if(this->dim(j) != other.dim(j))
return false;
return true;
}
/* ************************************************************************* */
VectorValues VectorValues::operator+(const VectorValues& c) const {
assert(varStarts_ == c.varStarts_);
VectorValues result;
result.varStarts_ = varStarts_;
result.values_ = values_.head(varStarts_.back())
+ c.values_.head(varStarts_.back());
assert(this->hasSameStructure(c));
VectorValues result(SameStructure(c));
result.values_ = this->values_ + c.values_;
return result;
}
/* ************************************************************************* */
void VectorValues::operator+=(const VectorValues& c) {
assert(varStarts_ == c.varStarts_);
this->values_ += c.values_.head(varStarts_.back());
assert(this->hasSameStructure(c));
this->values_ += c.values_;
}
/* ************************************************************************* */

599
gtsam/linear/VectorValues.h
View File

@ -22,364 +22,345 @@
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <numeric>
namespace gtsam {
/**
* The class VectorValues stores a number of Vectors.
* Typically, this class is used in back-substitution (Bayes Net solve).
*
* There are a number of constructors that simply reserve space
* (Frank is not a big fan of this imperative style, but it is needed for speed)
* and others - safer ones - that actually initialize values.
* SameStructure and reserve are two other utility functions that manage storage.
*
* Access is through the variable index j, and returns a SubVector,
* which is a view on the underlying data structure.
*/
class VectorValues {
protected:
Vector values_;
std::vector<size_t> varStarts_; // start at 0 with size nVars + 1
/**
* This class stores a collection of vector-valued variables, each referenced
* by a unique variable index. It is typically used to store the variables
* of a GaussianFactorGraph. Optimizing a GaussianFactorGraph or GaussianBayesNet
* returns this class.
*
* For basic usage, such as receiving a linear solution from gtsam solving functions,
* 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
* - Other facilities like iterators, size(), dim(), etc.
*
* Example:
* \code
VectorValues values;
values.insert(0, Vector_(3, 5.0, 6.0, 7.0));
values.insert(1, Vector_(2, 3.0, 4.0));
public:
// Forward declaration of iterator implementation
template<class C> class _impl_iterator;
typedef _impl_iterator<VectorValues> iterator;
typedef _impl_iterator<const VectorValues> const_iterator;
// Prints [ 3.0 4.0 ]
gtsam::print(values[1]);
// Some other typedefs
typedef boost::shared_ptr<VectorValues> shared_ptr;
typedef SubVector value_reference_type;
typedef ConstSubVector const_value_reference_type;
typedef SubVector mapped_type;
typedef ConstSubVector const_mapped_type;
// Prints [ 8.0 9.0 ]
values[1] = Vector_(2, 8.0, 9.0);
gtsam::print(values[1]);
\endcode
*
* Internally, this class stores all vectors as part of one large vector. This is
* necessary for performance, and the gtsam linear solving code exploits it by
* only allocating one large vector to store the solution. For advanced usage,
* or where speed is important, be aware of the following:
* - It is faster to allocate space ahead of time using a pre-allocating constructor
* or the resize() and append() functions, than to use insert(Index, const Vector&),
* which always has to re-allocate the internal vector.
* - The vector() function permits access to the underlying Vector.
* - operator[]() returns a SubVector view of the underlying Vector.
*
* Access is through the variable index j, and returns a SubVector,
* which is a view on the underlying data structure.
*
* This class is additionally used in gradient descent and dog leg to store the gradient.
* \nosubgrouping
*/
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_
/** Constructors that simply reserve space */
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 boost::shared_ptr<VectorValues> shared_ptr; ///< shared_ptr to this class
/**
* Default constructor creates an empty VectorValues. reserve(...) must be
* called to allocate space before any values can be added. This prevents
* slow reallocation of space at runtime.
*/
VectorValues() :
varStarts_(1, 0) {
}
/// @name Standard constructors
/// @{
/** Construct from a container of variable dimensions (in variable order). */
template<class CONTAINER>
VectorValues(const CONTAINER& dimensions);
/**
* Default constructor creates an empty VectorValues.
*/
VectorValues() {} //
/** Construct to hold nVars vectors of varDim dimension each. */
VectorValues(Index nVars, size_t varDim);
/** Copy constructor */
VectorValues(const VectorValues &other); //
/** Constructors that actually initialize values */
/** Named constructor to create a VectorValues of the same structure of the
* specifed one, but filled with zeros.
* @return
*/
static VectorValues Zero(const VectorValues& model);
/** Construct from a container of variable dimensions in variable order and
* a combined Vector of all of the variables in order.*/
VectorValues(const std::vector<size_t>& dimensions, const Vector& values);
/// @}
/// @name Standard interface
/// @{
/** Construct from the variable dimensions in varaible order and a double array that contains actual values */
VectorValues(const std::vector<size_t>& dimensions, const double* values);
/** 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(); } //
/** Copy constructor */
VectorValues(const VectorValues &V) :
values_(V.values_), varStarts_(V.varStarts_) {
}
/** Return the dimension of variable \c j. */
size_t dim(Index j) const { checkExists(j); return (*this)[j].rows(); } //
/** Named constructor to create a VectorValues that matches the structure of
* the specified VectorValues, but do not initialize the new values. */
static VectorValues SameStructure(const VectorValues& otherValues);
/** Return the summed dimensionality of all variables. */
size_t dim() const { return values_.rows(); } //
/** Reserve space for a total number of variables and dimensionality */
void reserve(Index nVars, size_t totalDims) {
values_.conservativeResize(totalDims);
varStarts_.reserve(nVars + 1);
}
/** Check whether a variable exists by index. */
bool exists(Index j) const { return j < size() && maps_[j].rows() > 0; } //
/** print required by Testable for unit testing */
void print(const std::string& str = "VectorValues: ") const;
/** Reference a variable by index. */
SubVector& operator[](Index j) { checkExists(j); return maps_[j]; } //
/** equals required by Testable for unit testing */
bool equals(const VectorValues& x, double tol = 1e-9) const;
/** Reference a variable by index. */
const SubVector& operator[](Index j) const { checkExists(j); return maps_[j]; } //
/** Number of elements */
Index size() const {
return varStarts_.size() - 1;
}
/** Insert a new variable (causes reallocation). */
void insert(Index j, const Vector& value); //
/** dimension of a particular element */
size_t dim(Index j) const;
/** Assignment */
VectorValues& operator=(const VectorValues& rhs); //
/** Total dimensionality used (could be smaller than what has been allocated
* with reserve(...) ).
*/
size_t dim() const {
return varStarts_.back();
}
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(); } ///< Iterator over variables
const_reverse_iterator rbegin() const { chk(); return maps_.rbegin(); } ///< Iterator over variables
reverse_iterator rend() { chk(); return maps_.rend(); } ///< Iterator over variables
const_reverse_iterator rend() const { chk(); return maps_.rend(); } ///< Iterator over variables
/** Total dimensions capacity allocated */
size_t dimCapacity() const {
return values_.size();
}
/** print required by Testable for unit testing */
void print(const std::string& str = "VectorValues: ") const; //
/** Reference the entire solution vector (const version). */
const Vector& vector() const {
return values_;
}
/** equals required by Testable for unit testing */
bool equals(const VectorValues& x, double tol = 1e-9) const; //
/** Reference the entire solution vector. */
Vector& vector() {
return values_;
}
/// @}
/// @name Advanced constructors
/// @{
/** Individual element access */
mapped_type operator[](Index j);
const_mapped_type operator[](Index j) const;
/** Construct from a container of variable dimensions (in variable order). */
template<class CONTAINER>
VectorValues(const CONTAINER& dimensions) { append(dimensions); } //
/** Iterator access */
iterator begin() {
return _impl_iterator<VectorValues>(*this, 0);
}
const_iterator begin() const {
return _impl_iterator<const VectorValues>(*this, 0);
}
iterator end() {
return _impl_iterator<VectorValues>(*this, varStarts_.size() - 1);
}
const_iterator end() const {
return _impl_iterator<const VectorValues>(*this, varStarts_.size() - 1);
}
/** Construct to hold nVars vectors of varDim dimension each. */
VectorValues(Index nVars, size_t varDim) { resize(nVars, varDim); } //
/** access a range of indices (of no particular order) as a single vector */
template<class ITERATOR>
Vector range(const ITERATOR& idx_begin, const ITERATOR& idx_end) const;
/** Named constructor to create a VectorValues that matches the structure of
* the specified VectorValues, but do not initialize the new values. */
static VectorValues SameStructure(const VectorValues& other); //
/** set a range of indices as a single vector split across the range */
template<class ITERATOR>
void range(const ITERATOR& idx_begin, const ITERATOR& idx_end,
const Vector& v);
/** Named constructor to create a VectorValues from a container of variable
* dimensions that is filled with zeros. */
template<class CONTAINER>
static VectorValues Zero(const CONTAINER& dimensions);
/** Set all elements to zero */
void makeZero() {
values_.setZero();
}
/// @}
/// @name Advanced interface
/// @{
/** Copy structure of x, but set all values to zero */
static VectorValues zero(const VectorValues& x);
/** 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 VectorValues& other); //
/**
* Append a variable using the next variable ID, and return that ID. Space
* must have been allocated ahead of time using reserve(...).
*/
Index push_back_preallocated(const Vector& vector);
/** Resize the VectorValues to hold \c nVars variables, each of dimension
* \c varDim. This function does not preserve any data, after calling
* it all variables 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);
/* dot product */
double dot(const VectorValues& V) const {
return gtsam::dot(this->values_, V.values_);
}
/** Resize the VectorValues to 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 does not
* preserve any data, after calling it all variables will be uninitialized.
* @param dimensions A container of the dimension of each variable to create.
*/
template<class CONTAINER>
void resize(const CONTAINER& dimensions);
/**
* + operator simply adds Vectors. This checks for structural equivalence
* when NDEBUG is not defined.
*/
VectorValues operator+(const VectorValues& c) const;
/** 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); //
void operator+=(const VectorValues& c);
/** Reference the entire solution vector (const version). */
const Vector& vector() const { chk(); return values_; } //
/**
* Iterator (handles both iterator and const_iterator depending on whether
* the template type is const.
*/
template<class C>
class _impl_iterator {
protected:
C& config_;
Index curVariable_;
/** Reference the entire solution vector. */
Vector& vector() { chk(); return values_; } //
_impl_iterator(C& config, Index curVariable) :
config_(config), curVariable_(curVariable) {
}
void checkCompat(const _impl_iterator<C>& r) {
assert(&config_ == &r.config_);
}
friend class VectorValues;
/** 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 VectorValues& other) const;
public:
typedef typename const_selector<C, VectorValues,
VectorValues::mapped_type, VectorValues::const_mapped_type>::type value_type;
_impl_iterator<C>& operator++() {
++curVariable_;
return *this;
}
_impl_iterator<C>& operator--() {
--curVariable_;
return *this;
}
_impl_iterator<C>& operator++(int) {
throw std::runtime_error("Use prefix ++ operator");
}
_impl_iterator<C>& operator--(int) {
throw std::runtime_error("Use prefix -- operator");
}
_impl_iterator<C>& operator+=(ptrdiff_t step) {
curVariable_ += step;
return *this;
}
_impl_iterator<C>& operator-=(ptrdiff_t step) {
curVariable_ += step;
return *this;
}
ptrdiff_t operator-(const _impl_iterator<C>& r) {
checkCompat(r);
return curVariable_ - r.curVariable_;
}
bool operator==(const _impl_iterator<C>& r) {
checkCompat(r);
return curVariable_ == r.curVariable_;
}
bool operator!=(const _impl_iterator<C>& r) {
checkCompat(r);
return curVariable_ != r.curVariable_;
}
value_type operator*() {
return config_[curVariable_];
}
};
/** 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_);
}
protected:
void checkVariable(Index j) const {
assert(j < varStarts_.size()-1);
}
/**
* + operator does element-wise addition. Both VectorValues must have the
* same structure (checked when NDEBUG is not defined).
*/
VectorValues operator+(const VectorValues& c) const;
public:
friend size_t dim(const VectorValues& V) {
return V.varStarts_.back();
}
friend double dot(const VectorValues& V1, const VectorValues& V2) {
return gtsam::dot(V1.values_, V2.values_);
}
friend void scal(double alpha, VectorValues& x) {
gtsam::scal(alpha, x.values_);
}
friend void axpy(double alpha, const VectorValues& x, VectorValues& y) {
gtsam::axpy(alpha, x.values_, y.values_);
}
friend void sqrt(VectorValues &x) {
Vector y = gtsam::esqrt(x.values_);
x.values_ = y;
}
/**
* += operator does element-wise addition. Both VectorValues must have the
* same structure (checked when NDEBUG is not defined).
*/
void operator+=(const VectorValues& c);
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 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];
}
private:
// Verifies that the underlying Vector is consistent with the collection of SubVectors
void chk() const;
// check whether there's a zero in the vector
friend bool anyZero(const VectorValues& x, double tol = 1e-5) {
bool flag = false;
size_t i = 0;
for (const double *v = x.values_.data(); i < (size_t) x.values_.size();
++v) {
if (*v < tol && *v > -tol) {
flag = true;
break;
}
++i;
}
return flag;
}
// Throw an exception if j does not exist
void checkExists(Index j) const {
chk();
if(!exists(j))
throw std::out_of_range("VectorValues: requested variable index is not in this VectorValues.");
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(values_);
ar & BOOST_SERIALIZATION_NVP(varStarts_);
}
};
// \class VectorValues definition
// Resize
void copyStructureFrom(const VectorValues& other);
/// Implementations of functions
public:
/// 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_);
}
/// 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_);
}
/// 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;
}
template<class CONTAINER>
inline VectorValues::VectorValues(const CONTAINER& dimensions) :
varStarts_(dimensions.size() + 1) {
varStarts_[0] = 0;
size_t varStart = 0;
Index var = 0;
BOOST_FOREACH(size_t dim, dimensions)
{
varStarts_[++var] = (varStart += dim);
}
values_.resize(varStarts_.back());
}
/// 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];
}
template<class ITERATOR>
inline Vector VectorValues::range(const ITERATOR& idx_begin,
const ITERATOR& idx_end) const {
// find the size of the vector to build
size_t s = 0;
for (ITERATOR it = idx_begin; it != idx_end; ++it)
s += dim(*it);
/// 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];
}
// assign vector
Vector result(s);
size_t start = 0;
for (ITERATOR it = idx_begin; it != idx_end; ++it) {
ConstSubVector v = (*this)[*it];
const size_t d = v.size();
result.segment(start, d).operator=(v); // This syntax works around what seems to be a bug in clang++
start += d;
}
return result;
}
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);
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
template<class ITERATOR>
inline void VectorValues::range(const ITERATOR& idx_begin,
const ITERATOR& idx_end, const Vector& v) {
size_t start = 0;
for (ITERATOR it = idx_begin; it != idx_end; ++it) {
checkVariable(*it);
const size_t d = dim(*it);
(*this)[*it] = v.segment(start, d);
start += d;
}
}
// Implementations of template and inline functions
struct DimSpec: public std::vector<size_t> {
/* ************************************************************************* */
template<class CONTAINER>
void VectorValues::resize(const CONTAINER& dimensions) {
maps_.clear();
values_.resize(0);
append(dimensions);
}
typedef std::vector<size_t> Base;
typedef boost::shared_ptr<DimSpec> shared_ptr;
/* ************************************************************************* */
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());
BOOST_FOREACH(size_t dim, dimensions) {
maps_.push_back(values_.segment(varStart, dim));
varStart += dim; // varStart is continued from first for loop
}
}
DimSpec() :
Base() {
}
DimSpec(size_t n) :
Base(n) {
}
DimSpec(size_t n, size_t init) :
Base(n, init) {
}
DimSpec(const VectorValues &V) :
Base(V.size()) {
const size_t n = V.size();
for (size_t i = 0; i < n; ++i) {
(*this)[i] = V[i].size();
}
}
};
/* ************************************************************************* */
template<class CONTAINER>
VectorValues VectorValues::Zero(const CONTAINER& dimensions) {
VectorValues ret(dimensions);
ret.vector() = Vector::Zero(ret.dim());
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());
}
#endif
}
} // \namespace gtsam

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

@ -28,51 +28,253 @@ using namespace boost::assign;
using namespace gtsam;
/* ************************************************************************* */
TEST(VectorValues, constructor) {
TEST(VectorValues, insert) {
// insert, with out-of-order indices
VectorValues actual;
actual.insert(0, Vector_(1, 1.0));
actual.insert(1, Vector_(2, 2.0, 3.0));
actual.insert(5, Vector_(2, 6.0, 7.0));
actual.insert(2, Vector_(2, 4.0, 5.0));
// 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));
LONGS_EQUAL(2, actual.dim(5));
// Logic
EXPECT(actual.exists(0));
EXPECT(actual.exists(1));
EXPECT(actual.exists(2));
EXPECT(!actual.exists(3));
EXPECT(!actual.exists(4));
EXPECT(actual.exists(5));
EXPECT(!actual.exists(6));
// Check values
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_(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()));
// Check exceptions
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
CHECK_EXCEPTION(actual.dim(3), out_of_range);
}
/* ************************************************************************* */
TEST(VectorValues, dimsConstructor) {
vector<size_t> dims;
dims.push_back(1);
dims.push_back(2);
dims.push_back(2);
double v[] = {1., 2., 3., 4., 5.};
VectorValues actual(dims, v);
VectorValues actual(dims);
actual[0] = Vector_(1, 1.0);
actual[1] = Vector_(2, 2.0, 3.0);
actual[2] = Vector_(2, 4.0, 5.0);
// Check dimensions
LONGS_EQUAL(3, actual.size());
DOUBLES_EQUAL(1., actual[0][0], 1e-15);
DOUBLES_EQUAL(2., actual[1][0], 1e-15);
DOUBLES_EQUAL(4., actual[2][0], 1e-15);
LONGS_EQUAL(5, actual.dim());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
// Check values
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()));
}
/* ************************************************************************* */
TEST(VectorValues, standard) {
Vector v1 = Vector_(3, 1.0,2.0,3.0);
Vector v2 = Vector_(2, 4.0,5.0);
Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0);
TEST(VectorValues, copyConstructor) {
vector<size_t> dims(3); dims[0]=3; dims[1]=2; dims[2]=4;
VectorValues combined(dims);
EXPECT_LONGS_EQUAL(3, combined.size());
EXPECT_LONGS_EQUAL(9, combined.dim());
EXPECT_LONGS_EQUAL(9, combined.dimCapacity());
EXPECT_LONGS_EQUAL(3, combined.dim(0));
EXPECT_LONGS_EQUAL(2, combined.dim(1));
EXPECT_LONGS_EQUAL(4, combined.dim(2));
combined[0] = v1;
combined[1] = v2;
combined[2] = v3;
// insert, with out-of-order indices
VectorValues original;
original.insert(0, Vector_(1, 1.0));
original.insert(1, Vector_(2, 2.0, 3.0));
original.insert(5, Vector_(2, 6.0, 7.0));
original.insert(2, Vector_(2, 4.0, 5.0));
CHECK(assert_equal(combined[0], v1))
CHECK(assert_equal(combined[1], v2))
CHECK(assert_equal(combined[2], v3))
VectorValues actual(original);
VectorValues incremental;
incremental.reserve(3, 9);
incremental.push_back_preallocated(v1);
incremental.push_back_preallocated(v2);
incremental.push_back_preallocated(v3);
// 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));
LONGS_EQUAL(2, actual.dim(5));
CHECK(assert_equal(incremental[0], v1))
CHECK(assert_equal(incremental[1], v2))
CHECK(assert_equal(incremental[2], v3))
// Logic
EXPECT(actual.exists(0));
EXPECT(actual.exists(1));
EXPECT(actual.exists(2));
EXPECT(!actual.exists(3));
EXPECT(!actual.exists(4));
EXPECT(actual.exists(5));
EXPECT(!actual.exists(6));
// Check values
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_(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()));
// Check exceptions
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
}
/* ************************************************************************* */
TEST(VectorValues, assignment) {
// insert, with out-of-order indices
VectorValues original;
original.insert(0, Vector_(1, 1.0));
original.insert(1, Vector_(2, 2.0, 3.0));
original.insert(5, Vector_(2, 6.0, 7.0));
original.insert(2, Vector_(2, 4.0, 5.0));
VectorValues actual = original;
// 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));
LONGS_EQUAL(2, actual.dim(5));
// Logic
EXPECT(actual.exists(0));
EXPECT(actual.exists(1));
EXPECT(actual.exists(2));
EXPECT(!actual.exists(3));
EXPECT(!actual.exists(4));
EXPECT(actual.exists(5));
EXPECT(!actual.exists(6));
// Check values
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_(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()));
// Check exceptions
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
}
/* ************************************************************************* */
TEST(VectorValues, SameStructure) {
// insert, with out-of-order indices
VectorValues original;
original.insert(0, Vector_(1, 1.0));
original.insert(1, Vector_(2, 2.0, 3.0));
original.insert(5, Vector_(2, 6.0, 7.0));
original.insert(2, Vector_(2, 4.0, 5.0));
VectorValues actual(VectorValues::SameStructure(original));
// 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));
LONGS_EQUAL(2, actual.dim(5));
// Logic
EXPECT(actual.exists(0));
EXPECT(actual.exists(1));
EXPECT(actual.exists(2));
EXPECT(!actual.exists(3));
EXPECT(!actual.exists(4));
EXPECT(actual.exists(5));
EXPECT(!actual.exists(6));
// Check exceptions
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
}
/* ************************************************************************* */
TEST(VectorValues, resizeLike) {
// insert, with out-of-order indices
VectorValues original;
original.insert(0, Vector_(1, 1.0));
original.insert(1, Vector_(2, 2.0, 3.0));
original.insert(5, Vector_(2, 6.0, 7.0));
original.insert(2, Vector_(2, 4.0, 5.0));
VectorValues actual(10, 3);
actual.resizeLike(original);
// 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));
LONGS_EQUAL(2, actual.dim(5));
// Logic
EXPECT(actual.exists(0));
EXPECT(actual.exists(1));
EXPECT(actual.exists(2));
EXPECT(!actual.exists(3));
EXPECT(!actual.exists(4));
EXPECT(actual.exists(5));
EXPECT(!actual.exists(6));
// Check exceptions
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
}
/* ************************************************************************* */
TEST(VectorValues, append) {
// insert
VectorValues actual;
actual.insert(0, Vector_(1, 1.0));
actual.insert(1, Vector_(2, 2.0, 3.0));
actual.insert(2, Vector_(2, 4.0, 5.0));
// append
vector<size_t> dims(2);
dims[0] = 3;
dims[1] = 5;
actual.append(dims);
// 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));
LONGS_EQUAL(3, actual.dim(3));
LONGS_EQUAL(5, actual.dim(4));
// Logic
EXPECT(actual.exists(0));
EXPECT(actual.exists(1));
EXPECT(actual.exists(2));
EXPECT(actual.exists(3));
EXPECT(actual.exists(4));
EXPECT(!actual.exists(5));
// Check values
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]));
// Check exceptions
CHECK_EXCEPTION(actual.insert(3, Vector()), invalid_argument);
}
/* ************************************************************************* */
@ -124,143 +326,52 @@ TEST(VectorValues, permuted_combined) {
}
/* ************************************************************************* */
TEST(VectorValues, permuted_incremental) {
Vector v1 = Vector_(3, 1.0,2.0,3.0);
Vector v2 = Vector_(2, 4.0,5.0);
Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0);
VectorValues incremental;
incremental.reserve(3, 9);
incremental.push_back_preallocated(v1);
incremental.push_back_preallocated(v2);
incremental.push_back_preallocated(v3);
Permutation perm1(3);
perm1[0] = 1;
perm1[1] = 2;
perm1[2] = 0;
Permutation perm2(3);
perm2[0] = 1;
perm2[1] = 2;
perm2[2] = 0;
Permuted<VectorValues> permuted1(incremental);
CHECK(assert_equal(v1, permuted1[0]))
CHECK(assert_equal(v2, permuted1[1]))
CHECK(assert_equal(v3, permuted1[2]))
permuted1.permute(perm1);
CHECK(assert_equal(v1, permuted1[2]))
CHECK(assert_equal(v2, permuted1[0]))
CHECK(assert_equal(v3, permuted1[1]))
permuted1.permute(perm2);
CHECK(assert_equal(v1, permuted1[1]))
CHECK(assert_equal(v2, permuted1[2]))
CHECK(assert_equal(v3, permuted1[0]))
Permuted<VectorValues> permuted2(perm1, incremental);
CHECK(assert_equal(v1, permuted2[2]))
CHECK(assert_equal(v2, permuted2[0]))
CHECK(assert_equal(v3, permuted2[1]))
permuted2.permute(perm2);
CHECK(assert_equal(v1, permuted2[1]))
CHECK(assert_equal(v2, permuted2[2]))
CHECK(assert_equal(v3, permuted2[0]))
}
/* ************************************************************************* */
TEST(VectorValues, makeZero ) {
VectorValues values(7, 2);
EXPECT_LONGS_EQUAL(14, values.dim());
EXPECT_LONGS_EQUAL(7, values.size());
EXPECT_LONGS_EQUAL(14, values.vector().size());
values.makeZero();
EXPECT(assert_equal(zero(14), values.vector()));
}
/* ************************************************************************* */
TEST(VectorValues, reserve ) {
Vector v1 = Vector_(3, 1.0,2.0,3.0);
Vector v2 = Vector_(2, 4.0,5.0);
Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0);
Vector v4(2); v4 << 10, 11;
Vector v5(3); v5 << 12, 13, 14;
// Expected has all 5 variables
vector<size_t> dimsExp(5); dimsExp[0]=3; dimsExp[1]=2; dimsExp[2]=4; dimsExp[3]=2; dimsExp[4]=3;
VectorValues expected(dimsExp);
expected[0] = v1;
expected[1] = v2;
expected[2] = v3;
expected[3] = v4;
expected[4] = v5;
// Start with 3 variables
vector<size_t> dims(3); dims[0]=3; dims[1]=2; dims[2]=4;
VectorValues actual(dims);
actual[0] = v1;
actual[1] = v2;
actual[2] = v3;
// Now expand to all 5
actual.reserve(5, 14);
actual.push_back_preallocated(v4);
actual.push_back_preallocated(v5);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(VectorValues, range ) {
VectorValues v(7,2);
v.makeZero();
v[1] = Vector_(2, 1.0, 2.0);
v[2] = Vector_(2, 3.0, 4.0);
v[3] = Vector_(2, 5.0, 6.0);
vector<size_t> idx1, idx2;
idx1 += 0, 1, 2, 3, 4, 5, 6; // ordered
idx2 += 1, 0, 2; // unordered
// test access
Vector actRange1 = v.range(idx1.begin(), idx1.begin() + 2);
EXPECT(assert_equal(Vector_(4, 0.0, 0.0, 1.0, 2.0), actRange1));
Vector actRange2 = v.range(idx1.begin()+1, idx1.begin()+2);
EXPECT(assert_equal(Vector_(2, 1.0, 2.0), actRange2));
Vector actRange3 = v.range(idx2.begin(), idx2.end());
EXPECT(assert_equal(Vector_(6, 1.0, 2.0, 0.0, 0.0, 3.0, 4.0), actRange3));
// test setting values
VectorValues act1 = v, act2 = v, act3 = v;
Vector a = Vector_(2, 0.1, 0.2);
VectorValues exp1 = act1; exp1[0] = a;
act1.range(idx1.begin(), idx1.begin()+1, a);
EXPECT(assert_equal(exp1, act1));
Vector bc = Vector_(4, 0.1, 0.2, 0.3, 0.4);
VectorValues exp2 = act2;
exp2[2] = Vector_(2, 0.1, 0.2);
exp2[3] = Vector_(2, 0.3, 0.4);
act2.range(idx1.begin()+2, idx1.begin()+4, bc);
EXPECT(assert_equal(exp2, act2));
Vector def = Vector_(6, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6);
VectorValues exp3 = act3;
exp3[1] = Vector_(2, 0.1, 0.2);
exp3[0] = Vector_(2, 0.3, 0.4);
exp3[2] = Vector_(2, 0.5, 0.6);
act3.range(idx2.begin(), idx2.end(), def);
EXPECT(assert_equal(exp3, act3));
}
///* ************************************************************************* */
//TEST(VectorValues, range ) {
// VectorValues v(7,2);
// v.makeZero();
// v[1] = Vector_(2, 1.0, 2.0);
// v[2] = Vector_(2, 3.0, 4.0);
// v[3] = Vector_(2, 5.0, 6.0);
//
// vector<size_t> idx1, idx2;
// idx1 += 0, 1, 2, 3, 4, 5, 6; // ordered
// idx2 += 1, 0, 2; // unordered
//
// // test access
//
// Vector actRange1 = v.range(idx1.begin(), idx1.begin() + 2);
// EXPECT(assert_equal(Vector_(4, 0.0, 0.0, 1.0, 2.0), actRange1));
//
// Vector actRange2 = v.range(idx1.begin()+1, idx1.begin()+2);
// EXPECT(assert_equal(Vector_(2, 1.0, 2.0), actRange2));
//
// Vector actRange3 = v.range(idx2.begin(), idx2.end());
// EXPECT(assert_equal(Vector_(6, 1.0, 2.0, 0.0, 0.0, 3.0, 4.0), actRange3));
//
// // test setting values
// VectorValues act1 = v, act2 = v, act3 = v;
//
// Vector a = Vector_(2, 0.1, 0.2);
// VectorValues exp1 = act1; exp1[0] = a;
// act1.range(idx1.begin(), idx1.begin()+1, a);
// EXPECT(assert_equal(exp1, act1));
//
// Vector bc = Vector_(4, 0.1, 0.2, 0.3, 0.4);
// VectorValues exp2 = act2;
// exp2[2] = Vector_(2, 0.1, 0.2);
// exp2[3] = Vector_(2, 0.3, 0.4);
// act2.range(idx1.begin()+2, idx1.begin()+4, bc);
// EXPECT(assert_equal(exp2, act2));
//
// Vector def = Vector_(6, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6);
// VectorValues exp3 = act3;
// exp3[1] = Vector_(2, 0.1, 0.2);
// exp3[0] = Vector_(2, 0.3, 0.4);
// exp3[2] = Vector_(2, 0.5, 0.6);
// act3.range(idx2.begin(), idx2.end(), def);
// EXPECT(assert_equal(exp3, act3));
//}
/* ************************************************************************* */
int main() {

2
gtsam/nonlinear/GaussianISAM2.cpp
View File

@ -62,7 +62,7 @@ void optimize2(const BayesTree<GaussianConditional>::sharedClique& clique, doubl
for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
if(!valuesChanged) {
const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]);
const VectorValues::mapped_type& newValue(delta[*it]);
const SubVector& newValue(delta[*it]);
if((oldValue - newValue).lpNorm<Eigen::Infinity>() >= threshold) {
valuesChanged = true;
break;

24
gtsam/nonlinear/ISAM2-impl-inl.h
View File

@ -125,14 +125,15 @@ struct ISAM2<CONDITIONAL, VALUES>::Impl {
struct _VariableAdder {
Ordering& ordering;
Permuted<VectorValues>& vconfig;
_VariableAdder(Ordering& _ordering, Permuted<VectorValues>& _vconfig) : ordering(_ordering), vconfig(_vconfig) {}
Index nextVar;
_VariableAdder(Ordering& _ordering, Permuted<VectorValues>& _vconfig, Index _nextVar) : ordering(_ordering), vconfig(_vconfig), nextVar(_nextVar){}
template<typename I>
void operator()(I xIt) {
const bool debug = ISDEBUG("ISAM2 AddVariables");
Index var = vconfig->push_back_preallocated(zero(xIt->second.dim()));
vconfig.permutation()[var] = var;
ordering.insert(xIt->first, var);
if(debug) cout << "Adding variable " << (string)xIt->first << " with order " << var << endl;
vconfig.permutation()[nextVar] = nextVar;
ordering.insert(xIt->first, nextVar);
if(debug) cout << "Adding variable " << (string)xIt->first << " with order " << nextVar << endl;
++ nextVar;
}
};
@ -145,15 +146,18 @@ void ISAM2<CONDITIONAL,VALUES>::Impl::AddVariables(
theta.insert(newTheta);
if(debug) newTheta.print("The new variables are: ");
// Add the new keys onto the ordering, add zeros to the delta for the new variables
vector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary(ordering.nVars())));
vector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary()));
if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl;
delta.container().reserve(delta->size() + newTheta.size(), delta->dim() + accumulate(dims.begin(), dims.end(), 0));
delta.permutation().resize(delta->size() + newTheta.size());
const size_t newDim = accumulate(dims.begin(), dims.end(), 0);
const size_t originalDim = delta->dim();
const size_t originalnVars = delta->size();
delta.container().append(dims);
delta.container().vector().segment(originalDim, newDim) = Vector::Zero(newDim);
delta.permutation().resize(originalnVars + newTheta.size());
{
_VariableAdder vadder(ordering, delta);
_VariableAdder vadder(ordering, delta, originalnVars);
newTheta.apply(vadder);
assert(delta.permutation().size() == delta.container().size());
assert(delta.container().dim() == delta.container().dimCapacity());
assert(ordering.nVars() == delta.size());
assert(ordering.size() == delta.size());
}

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

@ -77,9 +77,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
VectorValues LieValues<J>::zero(const Ordering& ordering) const {
VectorValues z(this->dims(ordering));
z.makeZero();
return z;
return VectorValues::Zero(this->dims(ordering));
}
/* ************************************************************************* */

8
gtsam/nonlinear/TupleValues.h
View File

@ -172,9 +172,7 @@ namespace gtsam {
/** zero: create VectorValues of appropriate structure */
VectorValues zero(const Ordering& ordering) const {
VectorValues z(this->dims(ordering));
z.makeZero();
return z;
return VectorValues::Zero(this->dims(ordering));
}
/** @return number of key/value pairs stored */
@ -313,9 +311,7 @@ namespace gtsam {
const Value1& at(const Key1& j) const { return first_.at(j); }
VectorValues zero(const Ordering& ordering) const {
VectorValues z(this->dims(ordering));
z.makeZero();
return z;
return VectorValues::Zero(this->dims(ordering));
}
size_t size() const { return first_.size(); }

6
gtsam/slam/tests/testPose2SLAM.cpp
View File

@ -352,16 +352,14 @@ TEST( Pose2Prior, error )
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(x0, ordering));
// Check error at x0, i.e. delta = zero !
VectorValues delta(x0.dims(ordering));
delta.makeZero();
VectorValues delta(VectorValues::Zero(x0.dims(ordering)));
delta[ordering["x1"]] = zero(3);
Vector error_at_zero = Vector_(3,0.0,0.0,0.0);
CHECK(assert_equal(error_at_zero,factor.whitenedError(x0)));
CHECK(assert_equal(-error_at_zero,linear->error_vector(delta)));
// Check error after increasing p2
VectorValues addition(x0.dims(ordering));
addition.makeZero();
VectorValues addition(VectorValues::Zero(x0.dims(ordering)));
addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0);
VectorValues plus = delta + addition;
Pose2Values x1 = x0.expmap(plus, ordering);

5
gtsam/slam/tests/testPose3SLAM.cpp
View File

@ -217,11 +217,12 @@ TEST( Pose3Values, expmap )
// so shifting to East requires little thought, different from with Pose2 !!!
Ordering ordering(*expected.orderingArbitrary());
VectorValues delta(expected.dims(ordering), Vector_(24,
VectorValues delta(expected.dims(ordering));
delta.vector() = Vector_(24,
0.0,0.0,0.0, 0.1, 0.0, 0.0,
0.0,0.0,0.0, 0.1, 0.0, 0.0,
0.0,0.0,0.0, 0.1, 0.0, 0.0,
0.0,0.0,0.0, 0.1, 0.0, 0.0));
0.0,0.0,0.0, 0.1, 0.0, 0.0);
Pose3Values actual = pose3SLAM::circle(4,1.0).expmap(delta, ordering);
CHECK(assert_equal(expected,actual));
}

6
tests/testGaussianISAM.cpp
View File

@ -70,8 +70,7 @@ TEST_UNSAFE( ISAM, iSAM_smoother )
EXPECT(assert_equal(expected, actual));
// obtain solution
VectorValues e(vector<size_t>(7,2)); // expected solution
e.makeZero();
VectorValues e(VectorValues::Zero(vector<size_t>(7,2))); // expected solution
VectorValues optimized = optimize(actual); // actual solution
EXPECT(assert_equal(e, optimized));
}
@ -182,8 +181,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_marginals )
// Create the Bayes tree
BayesTree<GaussianConditional> chordalBayesNet = *GaussianMultifrontalSolver(smoother).eliminate();
VectorValues expectedSolution(7, 2);
expectedSolution.makeZero();
VectorValues expectedSolution(VectorValues::Zero(vector<size_t>(7,2)));
VectorValues actualSolution = optimize(chordalBayesNet);
EXPECT(assert_equal(expectedSolution,actualSolution,tol));

12
tests/testGaussianISAM2.cpp
View File

@ -39,9 +39,8 @@ TEST(ISAM2, AddVariables) {
newTheta.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8));
VectorValues deltaUnpermuted;
deltaUnpermuted.reserve(2, 5);
{ Vector a(3); a << .1, .2, .3; deltaUnpermuted.push_back_preallocated(a); }
{ Vector a(2); a << .4, .5; deltaUnpermuted.push_back_preallocated(a); }
deltaUnpermuted.insert(0, Vector_(3, .1, .2, .3));
deltaUnpermuted.insert(1, Vector_(2, .4, .5));
Permutation permutation(2);
permutation[0] = 1;
@ -66,10 +65,9 @@ TEST(ISAM2, AddVariables) {
thetaExpected.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8));
VectorValues deltaUnpermutedExpected;
deltaUnpermutedExpected.reserve(3, 8);
{ Vector a(3); a << .1, .2, .3; deltaUnpermutedExpected.push_back_preallocated(a); }
{ Vector a(2); a << .4, .5; deltaUnpermutedExpected.push_back_preallocated(a); }
{ Vector a(3); a << 0, 0, 0; deltaUnpermutedExpected.push_back_preallocated(a); }
deltaUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3));
deltaUnpermutedExpected.insert(1, Vector_(2, .4, .5));
deltaUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
Permutation permutationExpected(3);
permutationExpected[0] = 1;

10
tests/testSerialization.cpp
View File

@ -314,12 +314,10 @@ TEST (Serialization, SharedDiagonal_noiseModels) {
/* ************************************************************************* */
TEST (Serialization, linear_factors) {
vector<size_t> dims;
dims.push_back(1);
dims.push_back(2);
dims.push_back(2);
double v[] = {1., 2., 3., 4., 5.};
VectorValues values(dims, v);
VectorValues values;
values.insert(0, Vector_(1, 1.0));
values.insert(1, Vector_(2, 2.0,3.0));
values.insert(2, Vector_(2, 4.0,5.0));
EXPECT(equalsObj<VectorValues>(values));
EXPECT(equalsXML<VectorValues>(values));