39 #ifndef MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_GREEDY_K_CENTERS_ 40 #define MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_GREEDY_K_CENTERS_ 44 #include <boost/numeric/ublas/matrix.hpp> 51 template <
typename _T>
58 using Matrix = boost::numeric::ublas::matrix<double>;
84 void kcenters(
const std::vector<_T>& data,
unsigned int k, std::vector<unsigned int>& centers,
Matrix& dists)
88 std::vector<double> minDist(data.size(), std::numeric_limits<double>::infinity());
92 if (dists.size1() < data.size() || dists.size2() < k)
93 dists.resize(std::max(2 * dists.size1() + 1, data.size()), k,
false);
95 centers.push_back(std::uniform_int_distribution<size_t>{ 0, data.size() - 1 }(
generator_));
96 for (
unsigned i = 1; i < k; ++i)
99 const _T& center = data[centers[i - 1]];
100 double maxDist = -std::numeric_limits<double>::infinity();
101 for (
unsigned j = 0; j < data.size(); ++j)
103 if ((dists(j, i - 1) =
distFun_(data[j], center)) < minDist[j])
104 minDist[j] = dists(j, i - 1);
106 if (minDist[j] > maxDist)
109 maxDist = minDist[j];
113 if (maxDist < std::numeric_limits<double>::epsilon())
115 centers.push_back(ind);
118 const _T& center = data[centers.back()];
119 unsigned i = centers.size() - 1;
120 for (
unsigned j = 0; j < data.size(); ++j)
121 dists(j, i) =
distFun_(data[j], center);
DistanceFunction distFun_
The used distance function.
virtual ~GreedyKCenters()=default
const DistanceFunction & getDistanceFunction() const
Get the distance function used.
void setDistanceFunction(const DistanceFunction &distFun)
Set the distance function to use.
An instance of this class can be used to greedily select a given number of representatives from a set...
std::function< double(const IKEntry *&, const IKEntry *&)> DistanceFunction
The definition of a distance function.
void kcenters(const std::vector< _T > &data, unsigned int k, std::vector< unsigned int > ¢ers, Matrix &dists)
Greedy algorithm for selecting k centers.
boost::numeric::ublas::matrix< double > Matrix
A matrix type for storing distances between points and centers.