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 <gtsam/linear/VectorValuesUnordered.h>
#include <boost/foreach.hpp>
#include <boost/range/combine.hpp> #include <boost/range/combine.hpp>
#include <boost/range/numeric.hpp> #include <boost/range/numeric.hpp>
#include <boost/range/adaptor/map.hpp>
#include <boost/range/adaptor/transformed.hpp> #include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/map.hpp>
using namespace std; using namespace std;
namespace gtsam { 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 { void VectorValuesUnordered::print(const std::string& str, const KeyFormatter& formatter) const {
std::cout << str << ": " << size() << " elements\n"; std::cout << str << ": " << size() << " elements\n";
@ -41,8 +61,8 @@ bool VectorValuesUnordered::equals(const VectorValuesUnordered& x, double tol) c
return false; return false;
typedef boost::tuple<value_type, value_type> ValuePair; typedef boost::tuple<value_type, value_type> ValuePair;
BOOST_FOREACH(const ValuePair& values, boost::combine(*this, x)) { BOOST_FOREACH(const ValuePair& values, boost::combine(*this, x)) {
if(values.get<0>.first != values.get<1>.first || if(values.get<0>().first != values.get<1>().first ||
!equal_with_abs_tol(values.get<0>.second, values.get<1>.second, tol)) !equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol))
return false; return false;
} }
return true; return true;
@ -51,11 +71,10 @@ bool VectorValuesUnordered::equals(const VectorValuesUnordered& x, double tol) c
/* ************************************************************************* */ /* ************************************************************************* */
const Vector VectorValuesUnordered::asVector() const const Vector VectorValuesUnordered::asVector() const
{ {
using boost::adaptors::map_values;
using boost::adaptors::transformed;
// Count dimensions // 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 // Copy vectors
Vector result; Vector result;
@ -104,7 +123,7 @@ double VectorValuesUnordered::dot(const VectorValuesUnordered& v) const
typedef boost::tuple<value_type, value_type> ValuePair; typedef boost::tuple<value_type, value_type> ValuePair;
using boost::adaptors::map_values; using boost::adaptors::map_values;
BOOST_FOREACH(const ValuePair& values, boost::combine(*this, v)) { 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")); std::invalid_argument("VectorValues::dot called with a VectorValues of different structure"));
assert_throw(values.get<0>().second.size() == values.get<1>().second.size(), assert_throw(values.get<0>().second.size() == values.get<1>().second.size(),
std::invalid_argument("VectorValues::dot called with a VectorValues of different structure")); 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 VectorValuesUnordered::operator+(const VectorValuesUnordered& c) const
VectorValuesUnordered result = SameStructure(*this); {
if(this->size() != c.size()) if(this->size() != c.size())
throw invalid_argument("VectorValues::operator+ called with different vector sizes"); throw invalid_argument("VectorValues::operator+ called with different vector sizes");
for(Index j = 0; j < this->size(); ++j) assert_throw(internal::hasSameStructure(*this, c),
// Directly accessing maps instead of using VV::dim in case some values are empty invalid_argument("VectorValues::operator+ called with different vector sizes"));
if(this->values_[j].size() == c.values_[j].size())
result.values_[j] = this->values_[j] + c.values_[j]; VectorValuesUnordered result;
else // The result.end() hint here should result in constant-time inserts
throw invalid_argument("VectorValues::operator- called with different vector sizes"); 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; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValuesUnordered VectorValuesUnordered::operator-(const VectorValuesUnordered& c) const { VectorValuesUnordered VectorValuesUnordered::operator-(const VectorValuesUnordered& c) const
VectorValuesUnordered result = SameStructure(*this); {
if(this->size() != c.size()) if(this->size() != c.size())
throw invalid_argument("VectorValues::operator- called with different vector sizes"); throw invalid_argument("VectorValues::operator- called with different vector sizes");
for(Index j = 0; j < this->size(); ++j) assert_throw(internal::hasSameStructure(*this, c),
// Directly accessing maps instead of using VV::dim in case some values are empty invalid_argument("VectorValues::operator- called with different vector sizes"));
if(this->values_[j].size() == c.values_[j].size())
result.values_[j] = this->values_[j] - c.values_[j]; VectorValuesUnordered result;
else // The result.end() hint here should result in constant-time inserts
throw invalid_argument("VectorValues::operator- called with different vector sizes"); 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; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void VectorValuesUnordered::operator+=(const VectorValuesUnordered& c) { VectorValuesUnordered& VectorValuesUnordered::operator+=(const VectorValuesUnordered& c)
{
if(this->size() != c.size()) if(this->size() != c.size())
throw invalid_argument("VectorValues::operator+= called with different vector sizes"); throw invalid_argument("VectorValues::operator+= called with different vector sizes");
for(Index j = 0; j < this->size(); ++j) assert_throw(internal::hasSameStructure(*this, c),
// Directly accessing maps instead of using VV::dim in case some values are empty invalid_argument("VectorValues::operator+= called with different vector sizes"));
if(this->values_[j].size() == c.values_[j].size())
this->values_[j] += c.values_[j]; iterator j1 = begin();
else const_iterator j2 = begin();
throw invalid_argument("VectorValues::operator+= called with different vector sizes"); // 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/base/FastMap.h>
#include <gtsam/global_includes.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 <boost/shared_ptr.hpp>
#include <numeric>
#include <stdexcept>
namespace gtsam { namespace gtsam {
@ -91,6 +86,7 @@ namespace gtsam {
*/ */
class GTSAM_EXPORT VectorValuesUnordered { class GTSAM_EXPORT VectorValuesUnordered {
protected: protected:
typedef VectorValuesUnordered This;
typedef FastMap<Key, 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 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 * += operator does element-wise addition. Both VectorValues must have the
* same structure (checked when NDEBUG is not defined). * 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 /// @name Matlab syntactic sugar for linear algebra operations
/// @{ /// @{
inline VectorValuesUnordered add(const VectorValuesUnordered& c) const { return *this + c; } //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 scale(const double a, const VectorValuesUnordered& c) const { return a * (*this); }
/// @} /// @}
/** /**
* scale a vector by a scalar * scale a vector by a scalar
*/ */
friend VectorValuesUnordered operator*(const double a, const VectorValuesUnordered &v) { //friend VectorValuesUnordered operator*(const double a, const VectorValuesUnordered &v) {
VectorValuesUnordered result = VectorValuesUnordered::SameStructure(v); // VectorValuesUnordered result = VectorValuesUnordered::SameStructure(v);
for(Key j = 0; j < v.size(); ++j) // for(Key j = 0; j < v.size(); ++j)
result.values_[j] = a * v.values_[j]; // result.values_[j] = a * v.values_[j];
return result; // return result;
} //}
/// TODO: linear algebra interface seems to have been added for SPCG. ///// TODO: linear algebra interface seems to have been added for SPCG.
friend void scal(double alpha, VectorValuesUnordered& x) { //friend void scal(double alpha, VectorValuesUnordered& x) {
for(Key j = 0; j < x.size(); ++j) // for(Key j = 0; j < x.size(); ++j)
x.values_[j] *= alpha; // x.values_[j] *= alpha;
} //}
/// TODO: linear algebra interface seems to have been added for SPCG. ///// TODO: linear algebra interface seems to have been added for SPCG.
friend void axpy(double alpha, const VectorValuesUnordered& x, VectorValuesUnordered& y) { //friend void axpy(double alpha, const VectorValuesUnordered& x, VectorValuesUnordered& y) {
if(x.size() != y.size()) // if(x.size() != y.size())
throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); // throw std::invalid_argument("axpy(VectorValues) called with different vector sizes");
for(Key j = 0; j < x.size(); ++j) // for(Key j = 0; j < x.size(); ++j)
if(x.values_[j].size() == y.values_[j].size()) // if(x.values_[j].size() == y.values_[j].size())
y.values_[j] += alpha * x.values_[j]; // y.values_[j] += alpha * x.values_[j];
else // else
throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); // throw std::invalid_argument("axpy(VectorValues) called with different vector sizes");
} //}
/// TODO: linear algebra interface seems to have been added for SPCG. ///// TODO: linear algebra interface seems to have been added for SPCG.
friend void sqrt(VectorValuesUnordered &x) { //friend void sqrt(VectorValuesUnordered &x) {
for(Key j = 0; j < x.size(); ++j) // for(Key j = 0; j < x.size(); ++j)
x.values_[j] = x.values_[j].cwiseSqrt(); // x.values_[j] = x.values_[j].cwiseSqrt();
} //}
/// TODO: linear algebra interface seems to have been added for SPCG. ///// TODO: linear algebra interface seems to have been added for SPCG.
friend void ediv(const VectorValuesUnordered& numerator, const VectorValuesUnordered& denominator, VectorValuesUnordered &result) { //friend void ediv(const VectorValuesUnordered& numerator, const VectorValuesUnordered& denominator, VectorValuesUnordered &result) {
if(numerator.size() != denominator.size() || numerator.size() != result.size()) // if(numerator.size() != denominator.size() || numerator.size() != result.size())
throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); // throw std::invalid_argument("ediv(VectorValues) called with different vector sizes");
for(Key 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()) // 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]); // result.values_[j] = numerator.values_[j].cwiseQuotient(denominator.values_[j]);
else // else
throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); // throw std::invalid_argument("ediv(VectorValues) called with different vector sizes");
} //}
/// TODO: linear algebra interface seems to have been added for SPCG. ///// TODO: linear algebra interface seems to have been added for SPCG.
friend void edivInPlace(VectorValuesUnordered& x, const VectorValuesUnordered& y) { //friend void edivInPlace(VectorValuesUnordered& x, const VectorValuesUnordered& y) {
if(x.size() != y.size()) // if(x.size() != y.size())
throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); // throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes");
for(Key j = 0; j < x.size(); ++j) // for(Key j = 0; j < x.size(); ++j)
if(x.values_[j].size() == y.values_[j].size()) // if(x.values_[j].size() == y.values_[j].size())
x.values_[j].array() /= y.values_[j].array(); // x.values_[j].array() /= y.values_[j].array();
else // else
throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); // throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes");
} //}
private: private:
/** Serialization function */ /** Serialization function */