From abf9b9c1b801ced49356735927bd31715e77137a Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 24 Feb 2015 11:45:38 +0100
Subject: [PATCH] Offset equality is within a threshold.
---
include/hpp/manipulation/graph/statistics.hh | 7 ++++-
src/graph/statistics.cc | 28 ++++++++++++++------
2 files changed, 26 insertions(+), 9 deletions(-)
diff --git a/include/hpp/manipulation/graph/statistics.hh b/include/hpp/manipulation/graph/statistics.hh
index 6dab426..5e62301 100644
--- a/include/hpp/manipulation/graph/statistics.hh
+++ b/include/hpp/manipulation/graph/statistics.hh
@@ -40,7 +40,7 @@ namespace hpp {
typedef ::hpp::statistics::Bin Parent;
typedef std::list <core::NodePtr_t> RoadmapNodes_t;
- LeafBin(const vector_t& v);
+ LeafBin(const vector_t& v, value_type* threshold_);
void push_back(const core::NodePtr_t& n);
@@ -61,6 +61,8 @@ namespace hpp {
RoadmapNodes_t nodes_;
+ value_type* thr_;
+
std::ostream& printValue (std::ostream& os) const;
};
@@ -124,6 +126,9 @@ namespace hpp {
private:
/// The constraint that creates the foliation.
ConstraintSetPtr_t constraint_;
+
+ /// Threshold used for equality between offset values.
+ value_type threshold_;
};
class HPP_MANIPULATION_DLLLOCAL NodeHistogram : public ::hpp::statistics::Statistics < NodeBin >
diff --git a/src/graph/statistics.cc b/src/graph/statistics.cc
index 841381a..036acbc 100644
--- a/src/graph/statistics.cc
+++ b/src/graph/statistics.cc
@@ -19,8 +19,8 @@
namespace hpp {
namespace manipulation {
namespace graph {
- LeafBin::LeafBin(const vector_t& v):
- value_(v), nodes_()
+ LeafBin::LeafBin(const vector_t& v, value_type* thr):
+ value_(v), nodes_(), thr_ (thr)
{}
void LeafBin::push_back(const core::NodePtr_t& n)
@@ -33,7 +33,7 @@ namespace hpp {
const vector_t& v = rhs.value ();
assert (value_.size() == v.size());
for (int p = 0; p < value_.size(); p++) {
- if (value_[p] != v[p])
+ if (std::abs (value_[p] - v[p]) >= *thr_)
return value_[p] < v[p];
}
return false;
@@ -41,7 +41,13 @@ namespace hpp {
bool LeafBin::operator==(const LeafBin& rhs) const
{
- return value_ == rhs.value();
+ const vector_t& v = rhs.value ();
+ assert (value_.size() == v.size());
+ for (int p = 0; p < value_.size(); p++) {
+ if (std::abs (value_[p] - v[p]) >= *thr_)
+ return false;
+ }
+ return true;
}
const vector_t& LeafBin::value () const
@@ -148,8 +154,13 @@ namespace hpp {
}
LeafHistogram::LeafHistogram (const ConstraintSetPtr_t& constraint) :
- constraint_ (constraint)
- {}
+ constraint_ (constraint), threshold_ (0)
+ {
+ ConfigProjectorPtr_t p = constraint_->configProjector ();
+ if (p) {
+ threshold_ = p->errorThreshold () / sqrt(p->rightHandSide ().size ());
+ }
+ }
void LeafHistogram::add (const core::NodePtr_t& n)
{
@@ -157,9 +168,10 @@ namespace hpp {
if (constraint_->configProjector ()) {
it = insert
(LeafBin (constraint_->configProjector ()->rightHandSideFromConfig
- (*n->configuration ())));
+ (*n->configuration ()),
+ &threshold_));
} else {
- it = insert (LeafBin (vector_t (0)));
+ it = insert (LeafBin (vector_t (0), &threshold_));
}
it->push_back (n);
if (numberOfObservations()%10 == 0) {
--
GitLab