35#ifndef CPPSRC_CORE_INCLUDE_COVERAGECONTROL_ALGORITHMS_NEAR_OPTIMAL_CVT_ALGORITHM_H_
36#define CPPSRC_CORE_INCLUDE_COVERAGECONTROL_ALGORITHMS_NEAR_OPTIMAL_CVT_ALGORITHM_H_
48#include "CoverageControl/extern/lsap/Hungarian.h"
59inline auto NearOptimalCVTAlgorithm(
int const num_tries,
60 int const max_iterations,
61 int const num_sites,
MapType const &map,
62 int const map_size,
double const res) {
66 std::srand(time(NULL));
69 std::vector<Voronoi> all_voronoi_cells;
72 all_voronoi_cells.resize(num_tries);
73 std::vector<double> obj_values;
74 obj_values.resize(num_tries, 0);
75 std::uniform_real_distribution<> distrib_pts(0.001, map_size * res - 0.001);
77#pragma omp parallel for
78 for (
int iter = 0; iter < num_tries; ++iter) {
80 sites.resize(num_sites);
81 for (
int iSite = 0; iSite < num_sites; ++iSite) {
82 sites[iSite] =
Point2(distrib_pts(gen_), distrib_pts(gen_));
84 bool cont_flag =
true;
86 Voronoi voronoi(sites, map,
Point2(map_size, map_size), res);
88 auto voronoi_cells = voronoi.GetVoronoiCells();
90 for (iSteps = 0; iSteps < max_iterations and cont_flag ==
true; ++iSteps) {
92 voronoi_cells = voronoi.GetVoronoiCells();
93 for (
int iSite = 0; iSite < num_sites; ++iSite) {
95 voronoi_cells[iSite].centroid() - voronoi_cells[iSite].site;
96 if (diff.norm() < res) {
100 sites[iSite] = voronoi_cells[iSite].centroid();
102 voronoi.UpdateSites(sites);
105 all_voronoi_cells[iter] = voronoi;
106 obj_values[iter] = voronoi.GetSumIDFGoalDistSqr();
108 int best_vornoi_idx = 0;
109 double min = obj_values[0];
110 for (
int iter = 1; iter < num_tries; ++iter) {
111 if (obj_values[iter] < min) {
112 min = obj_values[iter];
113 best_vornoi_idx = iter;
116 return all_voronoi_cells[best_vornoi_idx];
119inline auto NearOptimalCVTAlgorithm(
int const num_tries,
120 int const max_iterations,
121 int const num_sites,
MapType const &map,
122 int const map_size,
double const res,
123 PointVector
const &positions,
125 voronoi = NearOptimalCVTAlgorithm(num_tries, max_iterations, num_sites, map,
127 auto voronoi_cells = voronoi.GetVoronoiCells();
128 std::vector<std::vector<double>> cost_matrix;
129 cost_matrix.resize(num_sites, std::vector<double>(num_sites));
130#pragma omp parallel for num_threads(num_sites)
131 for (
int iRobot = 0; iRobot < num_sites; ++iRobot) {
132 for (
int jCentroid = 0; jCentroid < num_sites; ++jCentroid) {
133 cost_matrix[iRobot][jCentroid] =
134 (positions[iRobot] - voronoi_cells[jCentroid].centroid()).norm();
137 HungarianAlgorithm HungAlgo;
138 std::vector<int> assignment;
139 HungAlgo.Solve(cost_matrix, assignment);
142 goals.resize(num_sites);
143 for (
int iRobot = 0; iRobot < num_sites; ++iRobot) {
144 goals[iRobot] = voronoi_cells[assignment[iRobot]].centroid();
150inline auto NearOptimalCVTAlgorithm(
int const num_tries,
151 int const max_iterations,
152 int const num_sites,
MapType const &map,
153 int const map_size,
double const res,
154 PointVector
const &positions) {
156 return NearOptimalCVTAlgorithm(num_tries, max_iterations, num_sites, map,
157 map_size, res, positions, voronoi);
Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > MapType
Namespace for the CoverageControl library.
Contains typedefs for the library.