63 double sum_idf_site_dist_sqr_ = 0;
64 double sum_idf_goal_dist_sqr_ = 0;
65 double sum_idf_site_dist_ = 0;
66 double sum_idf_goal_dist_ = 0;
69 Point2 centroid()
const {
return centroid_; }
70 double mass()
const {
return mass_; }
71 double sum_idf_site_dist()
const {
return sum_idf_site_dist_; }
72 double sum_idf_site_dist_sqr()
const {
return sum_idf_site_dist_sqr_; }
73 double sum_idf_goal_dist()
const {
return sum_idf_goal_dist_; }
74 double sum_idf_goal_dist_sqr()
const {
return sum_idf_goal_dist_sqr_; }
76 auto GetFeatureVector()
const {
77 return std::vector<double>{
78 centroid_.x(), centroid_.y(), mass_,
79 sum_idf_site_dist_sqr_, sum_idf_goal_dist_sqr_, sum_idf_site_dist_,
86 origin_shift =
Point2{0, 0};
87 sum_idf_site_dist_ = 0;
88 sum_idf_site_dist_sqr_ = 0;
89 sum_idf_goal_dist_ = 0;
90 sum_idf_goal_dist_sqr_ = 0;
92 void MassCentroidFunctional(
double const &map_val,
Point2 pt) {
93 pt = pt + origin_shift;
95 centroid_ += pt * map_val;
96 sum_idf_site_dist_ += (pt - site).norm() * map_val;
97 sum_idf_site_dist_sqr_ += (pt - site).squaredNorm() * map_val;
99 void GoalObjFunctional(
double const &map_val,
Point2 pt) {
100 pt = pt + origin_shift;
101 sum_idf_goal_dist_sqr_ += (pt - centroid_).squaredNorm() * map_val;
102 sum_idf_goal_dist_ += (pt - centroid_).norm() * map_val;
105 void ComputeFinalCentroid();
122 std::shared_ptr<const MapType> map_ =
nullptr;
124 double resolution_ = 0;
131 bool compute_single_ =
false;
136 std::vector<VoronoiCell> voronoi_cells_;
139 void MassCentroidFunctional(
VoronoiCell &vcell,
double const &map_val,
147 double const &resolution,
bool const compute_single =
false,
148 int const robot_id = 0)
151 resolution_{resolution},
152 compute_single_{compute_single},
153 robot_id_{robot_id} {
156 map_ = std::make_shared<const MapType>(map);
157 num_sites_ = sites_.size();
171 if (compute_single_ ==
false) {
172 voronoi_cells_.resize(num_sites_);
174 origin_shift_ = -sites[0];
177 ComputeVoronoiCells();
183 num_sites_ = sites_.size();
184 ComputeVoronoiCells();
187 void ComputeVoronoiCells();
188 auto GetVoronoiCells()
const {
return voronoi_cells_; }
189 auto GetVoronoiCell() {
return voronoi_cell_; }
191 double GetSumIDFSiteDistSqr() {
193 for (
auto const &cell : voronoi_cells_) {
194 obj = obj + cell.sum_idf_site_dist_sqr();
198 double GetSumIDFGoalDistSqr() {
200 for (
auto const &cell : voronoi_cells_) {
201 obj = obj + cell.sum_idf_goal_dist_sqr();