inline T Histogram<T>::getPeek() const {
std::vector<size_t> numberSamplesPerBin;
std::vector<T> lowerBounds = getLowerBounds();
std::vector<T> upperBounds = getUpperBounds();
if(lowerBounds.size() != upperBounds.size()) {ROS_ERROR_STREAM("NOT SAME SIZE" << "lower.size: " << lowerBounds.size() << " upper.size: " << upperBounds.size()); return 0;}
size_t max_at_index = 0;
for(size_t i = 0; i < numberBins; ++i) {
numberSamplesPerBin.push_back( getNumberSamplesBetween(lowerBounds[i], upperBounds[i]) );
if(i > 0 && numberSamplesPerBin[i - 1] < numberSamplesPerBin[i]) max_at_index = i;
}
size_t distanceToMiddle = numberSamplesPerBin[max_at_index] / 2;
typename std::multiset<T>::const_iterator it = values.lower_bound(lowerBounds[max_at_index]);
for(size_t j = 0; j < distanceToMiddle; ++j) ++it;
return *it;
}
SimulatedAnnealing::SimulatedAnnealing(
const std::shared_ptr<OptimisationProblem> optimisationProblem)
: TrajectoryBasedOptimisationAlgorithm(optimisationProblem) {
setMaximalStepSize((getUpperBounds() - getLowerBounds()) * 0.1);
}
