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

Cleanups and standardizing linear algebra functions in VectorValuesUnordered

release/4.3a0
Richard Roberts 2013-07-12 22:27:46 +00:00
parent d16fbc52eb
commit 9f44492380
2 changed files with 91 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
gtsam/linear/VectorValuesUnordered.cpp
View File

@ -34,20 +34,6 @@ namespace gtsam {
using boost::adaptors::map_values;
using boost::accumulate;
/* ************************************************************************* */
namespace internal
{
bool structureCompareOp(const boost::tuple<VectorValuesUnordered::value_type, VectorValuesUnordered::value_type>& vv)
{
return vv.get<0>().first == vv.get<1>().first && vv.get<0>().second.size() == vv.get<1>().second.size();
}
bool hasSameStructure(const VectorValuesUnordered& vv1, const VectorValuesUnordered& vv2)
{
return accumulate(combine(vv1, vv2) | transformed(structureCompareOp), true, std::logical_and<bool>());
}
}
/* ************************************************************************* */
VectorValuesUnordered::VectorValuesUnordered(const VectorValuesUnordered& first, const VectorValuesUnordered& second)
{
@ -98,7 +84,7 @@ namespace gtsam {
}
/* ************************************************************************* */
const Vector VectorValuesUnordered::asVector() const
const Vector VectorValuesUnordered::vector() const
{
// Count dimensions
DenseIndex totalDim = 0;
@ -143,6 +129,24 @@ namespace gtsam {
this->values_.swap(other.values_);
}
/* ************************************************************************* */
namespace internal
{
bool structureCompareOp(const boost::tuple<VectorValuesUnordered::value_type,
VectorValuesUnordered::value_type>& vv)
{
return vv.get<0>().first == vv.get<1>().first
&& vv.get<0>().second.size() == vv.get<1>().second.size();
}
}
/* ************************************************************************* */
bool VectorValuesUnordered::hasSameStructure(const VectorValuesUnordered other) const
{
return accumulate(combine(*this, other)
| transformed(internal::structureCompareOp), true, std::logical_and<bool>());
}
/* ************************************************************************* */
double VectorValuesUnordered::dot(const VectorValuesUnordered& v) const
{
@ -180,7 +184,7 @@ namespace gtsam {
{
if(this->size() != c.size())
throw invalid_argument("VectorValues::operator+ called with different vector sizes");
assert_throw(internal::hasSameStructure(*this, c),
assert_throw(hasSameStructure(c),
invalid_argument("VectorValues::operator+ called with different vector sizes"));
VectorValuesUnordered result;
@ -191,12 +195,41 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
VectorValuesUnordered VectorValuesUnordered::add(const VectorValuesUnordered& c) const
{
return *this + c;
}
/* ************************************************************************* */
VectorValuesUnordered& VectorValuesUnordered::operator+=(const VectorValuesUnordered& c)
{
if(this->size() != c.size())
throw invalid_argument("VectorValues::operator+= called with different vector sizes");
assert_throw(hasSameStructure(c),
invalid_argument("VectorValues::operator+= called with different vector sizes"));
iterator j1 = begin();
const_iterator j2 = c.begin();
// The result.end() hint here should result in constant-time inserts
for(; j1 != end(); ++j1, ++j2)
j1->second += j2->second;
return *this;
}
/* ************************************************************************* */
VectorValuesUnordered& VectorValuesUnordered::addInPlace(const VectorValuesUnordered& c)
{
return *this += c;
}
/* ************************************************************************* */
VectorValuesUnordered VectorValuesUnordered::operator-(const VectorValuesUnordered& c) const
{
if(this->size() != c.size())
throw invalid_argument("VectorValues::operator- called with different vector sizes");
assert_throw(internal::hasSameStructure(*this, c),
assert_throw(hasSameStructure(c),
invalid_argument("VectorValues::operator- called with different vector sizes"));
VectorValuesUnordered result;
@ -208,27 +241,23 @@ namespace gtsam {
}
/* ************************************************************************* */
VectorValuesUnordered& VectorValuesUnordered::operator+=(const VectorValuesUnordered& c)
VectorValuesUnordered VectorValuesUnordered::subtract(const VectorValuesUnordered& c) const
{
if(this->size() != c.size())
throw invalid_argument("VectorValues::operator+= called with different vector sizes");
assert_throw(internal::hasSameStructure(*this, c),
invalid_argument("VectorValues::operator+= called with different vector sizes"));
iterator j1 = begin();
const_iterator j2 = begin();
// The result.end() hint here should result in constant-time inserts
for(; j1 != end(); ++j1, ++j2)
j1->second += j2->second;
return *this - c;
}
/* ************************************************************************* */
VectorValuesUnordered& VectorValuesUnordered::operator*=(double alpha)
{
BOOST_FOREACH(Vector& v, *this | map_values)
v *= alpha;
return *this;
}
/* ************************************************************************* */
void scal(double alpha, VectorValuesUnordered& x)
VectorValuesUnordered& VectorValuesUnordered::scaleInPlace(double alpha)
{
BOOST_FOREACH(Vector& v, x | map_values)
v *= alpha;
return *this *= alpha;
}
/* ************************************************************************* */

53
gtsam/linear/VectorValuesUnordered.h
View File

@ -95,7 +95,7 @@ namespace gtsam {
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<VectorValuesUnordered> shared_ptr; ///< shared_ptr to this class
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
typedef Values::value_type value_type; ///< Typedef to pair<Key, Vector>, a key-value pair
typedef value_type KeyValuePair; ///< Typedef to pair<Key, Vector>, a key-value pair
@ -193,16 +193,17 @@ namespace gtsam {
/// @{
/** Retrieve the entire solution as a single vector */
const Vector asVector() const;
const Vector vector() 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.
*/
/** Swap the data in this VectorValues with another. */
void swap(VectorValuesUnordered& other);
/** Check if this VectorValues has the same structure (keys and dimensions) as another */
bool hasSameStructure(const VectorValuesUnordered other) const;
/// @}
/// @name Linear algebra operations
/// @{
@ -218,23 +219,35 @@ namespace gtsam {
/** Squared vector L2 norm */
double squaredNorm() const;
/**
* + operator does element-wise addition. Both VectorValues must have the
* same structure (checked when NDEBUG is not defined).
*/
/** Element-wise addition, synonym for add(). Both VectorValues must have the same structure
* (checked when NDEBUG is not defined). */
VectorValuesUnordered operator+(const VectorValuesUnordered& c) const;
/**
* + operator does element-wise subtraction. Both VectorValues must have the
* same structure (checked when NDEBUG is not defined).
*/
/** Element-wise addition, synonym for operator+(). Both VectorValues must have the same
* structure (checked when NDEBUG is not defined). */
VectorValuesUnordered add(const VectorValuesUnordered& c) const;
/** Element-wise addition in-place, synonym for operator+=(). Both VectorValues must have the
* same structure (checked when NDEBUG is not defined). */
VectorValuesUnordered& operator+=(const VectorValuesUnordered& c);
/** Element-wise addition in-place, synonym for operator+=(). Both VectorValues must have the
* same structure (checked when NDEBUG is not defined). */
VectorValuesUnordered& addInPlace(const VectorValuesUnordered& c);
/** Element-wise subtraction, synonym for subtract(). Both VectorValues must have the same
* structure (checked when NDEBUG is not defined). */
VectorValuesUnordered operator-(const VectorValuesUnordered& c) const;
/**
* += operator does element-wise addition. Both VectorValues must have the
* same structure (checked when NDEBUG is not defined).
*/
VectorValuesUnordered& operator+=(const VectorValuesUnordered& c);
/** Element-wise subtraction, synonym for operator-(). Both VectorValues must have the same
* structure (checked when NDEBUG is not defined). */
VectorValuesUnordered subtract(const VectorValuesUnordered& c) const;
/** Element-wise scaling by a constant in-place. */
VectorValuesUnordered& operator*=(double alpha);
/** Element-wise scaling by a constant in-place. */
VectorValuesUnordered& scaleInPlace(double alpha);
/// @}
@ -242,7 +255,6 @@ namespace gtsam {
/// @name Matlab syntactic sugar for linear algebra operations
/// @{
//inline VectorValuesUnordered add(const VectorValuesUnordered& c) const { return *this + c; }
//inline VectorValuesUnordered scale(const double a, const VectorValuesUnordered& c) const { return a * (*this); }
/// @}
@ -257,9 +269,6 @@ namespace gtsam {
// return result;
//}
// TODO: linear algebra interface seems to have been added for SPCG.
friend void scal(double alpha, VectorValuesUnordered& x);
//// 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())