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 15:33:06 +00:00
parent 530bde404e
commit 4d6647ba01
2 changed files with 109 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
gtsam/linear/VectorValuesUnordered.cpp
View File

@ -18,15 +18,35 @@
#include <gtsam/linear/VectorValuesUnordered.h>
#include <boost/foreach.hpp>
#include <boost/range/combine.hpp>
#include <boost/range/numeric.hpp>
#include <boost/range/adaptor/map.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/map.hpp>
using namespace std;
namespace gtsam {
using boost::combine;
using boost::adaptors::transformed;
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>());
}
}
/* ************************************************************************* */
void VectorValuesUnordered::print(const std::string& str, const KeyFormatter& formatter) const {
std::cout << str << ": " << size() << " elements\n";
@ -41,8 +61,8 @@ bool VectorValuesUnordered::equals(const VectorValuesUnordered& x, double tol) c
return false;
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))
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;
@ -51,11 +71,10 @@ bool VectorValuesUnordered::equals(const VectorValuesUnordered& x, double tol) c
/* ************************************************************************* */
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);
DenseIndex totalDim = 0;
BOOST_FOREACH(const value_type& v, *this)
totalDim += v.second.size();
// Copy vectors
Vector result;
@ -104,7 +123,7 @@ double VectorValuesUnordered::dot(const VectorValuesUnordered& v) const
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,
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"));
@ -128,43 +147,52 @@ double VectorValuesUnordered::squaredNorm() const {
}
/* ************************************************************************* */
VectorValuesUnordered VectorValuesUnordered::operator+(const VectorValuesUnordered& c) const {
VectorValuesUnordered result = SameStructure(*this);
VectorValuesUnordered VectorValuesUnordered::operator+(const VectorValuesUnordered& c) const
{
if(this->size() != c.size())
throw invalid_argument("VectorValues::operator+ called with different vector sizes");
for(Index j = 0; j < this->size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
if(this->values_[j].size() == c.values_[j].size())
result.values_[j] = this->values_[j] + c.values_[j];
else
throw invalid_argument("VectorValues::operator- called with different vector sizes");
assert_throw(internal::hasSameStructure(*this, c),
invalid_argument("VectorValues::operator+ called with different vector sizes"));
VectorValuesUnordered result;
// The result.end() hint here should result in constant-time inserts
for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2)
result.values_.insert(result.end(), make_pair(j1->first, j1->second + j2->second));
return result;
}
/* ************************************************************************* */
VectorValuesUnordered VectorValuesUnordered::operator-(const VectorValuesUnordered& c) const {
VectorValuesUnordered result = SameStructure(*this);
VectorValuesUnordered VectorValuesUnordered::operator-(const VectorValuesUnordered& c) const
{
if(this->size() != c.size())
throw invalid_argument("VectorValues::operator- called with different vector sizes");
for(Index j = 0; j < this->size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
if(this->values_[j].size() == c.values_[j].size())
result.values_[j] = this->values_[j] - c.values_[j];
else
throw invalid_argument("VectorValues::operator- called with different vector sizes");
assert_throw(internal::hasSameStructure(*this, c),
invalid_argument("VectorValues::operator- called with different vector sizes"));
VectorValuesUnordered result;
// The result.end() hint here should result in constant-time inserts
for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2)
result.values_.insert(result.end(), make_pair(j1->first, j1->second - j2->second));
return result;
}
/* ************************************************************************* */
void VectorValuesUnordered::operator+=(const VectorValuesUnordered& c) {
VectorValuesUnordered& VectorValuesUnordered::operator+=(const VectorValuesUnordered& c)
{
if(this->size() != c.size())
throw invalid_argument("VectorValues::operator+= called with different vector sizes");
for(Index j = 0; j < this->size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
if(this->values_[j].size() == c.values_[j].size())
this->values_[j] += c.values_[j];
else
throw invalid_argument("VectorValues::operator+= called with different vector sizes");
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;
}
/* ************************************************************************* */

104
gtsam/linear/VectorValuesUnordered.h
View File

@ -21,12 +21,7 @@
#include <gtsam/base/FastMap.h>
#include <gtsam/global_includes.h>
#include <boost/format.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <numeric>
#include <stdexcept>
namespace gtsam {
@ -91,6 +86,7 @@ namespace gtsam {
*/
class GTSAM_EXPORT VectorValuesUnordered {
protected:
typedef VectorValuesUnordered This;
typedef FastMap<Key, Vector> Values; ///< Typedef for the collection of Vectors making up a VectorValues
Values values_; ///< Collection of Vectors making up this VectorValues
@ -227,7 +223,7 @@ namespace gtsam {
* += operator does element-wise addition. Both VectorValues must have the
* same structure (checked when NDEBUG is not defined).
*/
void operator+=(const VectorValuesUnordered& c);
VectorValuesUnordered& operator+=(const VectorValuesUnordered& c);
/// @}
@ -235,63 +231,63 @@ 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); }
//inline VectorValuesUnordered add(const VectorValuesUnordered& c) const { return *this + c; }
//inline VectorValuesUnordered scale(const double a, const VectorValuesUnordered& c) const { return a * (*this); }
/// @}
/**
* scale a vector by a scalar
*/
friend VectorValuesUnordered operator*(const double a, const VectorValuesUnordered &v) {
VectorValuesUnordered result = VectorValuesUnordered::SameStructure(v);
for(Key j = 0; j < v.size(); ++j)
result.values_[j] = a * v.values_[j];
return result;
}
//friend VectorValuesUnordered operator*(const double a, const VectorValuesUnordered &v) {
// VectorValuesUnordered result = VectorValuesUnordered::SameStructure(v);
// 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(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(Key j = 0; j < x.size(); ++j)
if(x.values_[j].size() == y.values_[j].size())
y.values_[j] += alpha * x.values_[j];
else
throw std::invalid_argument("axpy(VectorValues) called with different vector sizes");
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend void sqrt(VectorValuesUnordered &x) {
for(Key j = 0; j < x.size(); ++j)
x.values_[j] = x.values_[j].cwiseSqrt();
}
///// TODO: linear algebra interface seems to have been added for SPCG.
//friend void scal(double alpha, VectorValuesUnordered& x) {
// 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(Key j = 0; j < x.size(); ++j)
// if(x.values_[j].size() == y.values_[j].size())
// y.values_[j] += alpha * x.values_[j];
// else
// throw std::invalid_argument("axpy(VectorValues) called with different vector sizes");
//}
///// TODO: linear algebra interface seems to have been added for SPCG.
//friend void sqrt(VectorValuesUnordered &x) {
// for(Key j = 0; j < x.size(); ++j)
// x.values_[j] = x.values_[j].cwiseSqrt();
//}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend void ediv(const 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(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
throw std::invalid_argument("ediv(VectorValues) called with different vector sizes");
}
///// TODO: linear algebra interface seems to have been added for SPCG.
//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(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
// throw std::invalid_argument("ediv(VectorValues) called with different vector sizes");
//}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend void edivInPlace(VectorValuesUnordered& x, const VectorValuesUnordered& y) {
if(x.size() != y.size())
throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes");
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
throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes");
}
///// TODO: linear algebra interface seems to have been added for SPCG.
//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(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
// throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes");
//}
private:
/** Serialization function */