lib/area_division.cpp
Go to the documentation of this file.
1 #include "lib/area_division.h"
2 
4 {
5  // read parameters
6  NodeHandle nh;
7  nh.param(this_node::getName() + "/optimizer/iterations", max_iter, 10);
8  nh.param(this_node::getName() + "/optimizer/variate_weight", variate_weight, 0.01);
9  nh.param(this_node::getName() + "/optimizer/discrepancy", discr, 30);
10 }
11 
13 {
14  // initializations
15  int NoTiles = rows*cols;
16  double fairDivision = 1.0 / nr;
17  int effectiveSize = NoTiles - nr - ob;
18  int termThr;
19  if (effectiveSize % nr !=0) {
20  termThr=1;
21  }
22  else {
23  termThr=0;
24  }
25 
26  // initialize distances of cells to cps
27  vector<valarray<double>> AllDistances(nr, valarray<double>(rows*cols));
28  for (int i=0; i<rows; i++) {
29  for (int j=0; j<cols;j++) {
30  for (int r=0; r<nr; r++) {
31  AllDistances[r][i*cols+j] = hypot(cps[r][1]-i, cps[r][0]-j);
32  }
33  }
34  }
35 
36 
37  vector<valarray<double>> MetricMatrix = AllDistances;
38 
39  // perform area division
40  success = false;
41  while (termThr<=discr && !success) {
42  // initializations
43  double downThres = ((double)NoTiles-(double)termThr*(nr-1)) / (double)(NoTiles*nr);
44  double upperThres = ((double)NoTiles+termThr) / (double)(NoTiles*nr);
45  success = true;
46 
47  // main optimization loop
48  int iter = 0;
49  while (iter <= max_iter) {
50  assign(MetricMatrix);
51 
52  // find connected areas
53  vector<valarray<float>> ConnectedMultiplierList(nr);
54  double plainErrors[nr];
55  double divFairError[nr];
56  for (int r=0; r<nr; r++) {
57  valarray<float> ConnectedMultiplier(1, rows*cols);
58  regions[r] = true;
59 
60  connected_components cc(BWlist[r], rows, cols, true);
61  valarray<int> Ilabel = cc.compactLabeling();
62  // at least one unconnected regions among r-robot's regions is found
63  if (cc.getMaxLabel() > 1) {
64  regions[r] = false;
65 
66  // find robot's sub-region and construct robot and non-robot binary regions
67  cc.constructBinaryImages(Ilabel[cps[r][1] * cols + cps[r][0]]);
68 
69  // construct the final connected component multiplier
71  }
72  ConnectedMultiplierList[r] = ConnectedMultiplier;
73 
74  // calculate the deviation from the the optimal assignment
75  plainErrors[r] = ArrayOfElements[r] / (double) effectiveSize;
76  if (plainErrors[r] < downThres) {
77  divFairError[r] = downThres - plainErrors[r];
78  }
79  else if (plainErrors[r] > upperThres) {
80  divFairError[r] = upperThres - plainErrors[r];
81  }
82  }
83 
84  // exit conditions
85  if (isThisAGoalState(termThr)) {
86  break;
87  }
88 
89  // check fairness among different partitions
90  double TotalNegPerc = 0.0, totalNegPlainErrors = 0.0;
91  double correctionMult[nr];
92  for (int r=0; r<nr; r++) {
93  if (divFairError[r] < 0) {
94  TotalNegPerc += abs(divFairError[r]);
95  totalNegPlainErrors += plainErrors[r];
96  }
97  correctionMult[r] = 1.0;
98  }
99 
100  // restore fairness
101  for (int r=0; r<nr; r++) {
102  if (totalNegPlainErrors != 0.0) {
103  if (divFairError[r]<0.0) {
104  correctionMult[r] = 1.0 + (plainErrors[r] / totalNegPlainErrors) * (TotalNegPerc / 2.0);
105  }
106  else {
107  correctionMult[r] = 1.0 - (plainErrors[r] / totalNegPlainErrors) * (TotalNegPerc / 2.0);
108  }
109  }
110  MetricMatrix[r] = FinalUpdateOnMetricMatrix(correctionMult[r], MetricMatrix[r], ConnectedMultiplierList[r]);
111  }
112 
113  iter++;
114  }
115 
116  // could not find area division
117  if (iter >= max_iter) {
118  max_iter = max_iter/2;
119  success = false;
120 
121  // increase allowed area discrepancy
122  termThr++;
123  }
124  }
125 
126  if (success == false)
127  ROS_ERROR("Area division failed!");
128 }
129 
130 nav_msgs::OccupancyGrid area_division::get_grid (nav_msgs::OccupancyGrid map, string cps)
131 {
132  nav_msgs::OccupancyGrid assigned;
133  assigned = map;
134 
135  for (int i=0; i<rows; i++) {
136  for (int j=0; j<cols;j++) {
137  // mark assigned cells as free
138  if (A[(rows-1-i)*cols+j] == uuid_map[cps]) { // origin of division algorithm is top left
139  assigned.data[i*cols+j] = 0;
140  }
141 
142  // mark cells assigned to other cpss as obstacles
143  else {
144  assigned.data[i*cols+j] = 100;
145  }
146  }
147  }
148 
149  assigned.header.stamp = Time::now();
150  assigned.info.map_load_time == Time::now();
151 
152  return assigned;
153 }
154 
155 void area_division::initialize_cps (map<string, vector<int>> cpss)
156 {
157  // initialize
158  nr = 0;
159  cps.clear();
160  uuid_map.clear();
161  A.resize(rows*cols);
162  regions.clear();
163  regions.resize(cpss.size());
164 
165  // place cpss in the map
166  for (auto c : cpss) {
167  // divison algorithm assumes origin at top left
168  int x = c.second[0];
169  int y = rows - 1 - c.second[1];
170 
171  // index of position in grid map
172  int idx = y * cols + x;
173 
174  // place cps in data structures
175  gridmap[idx] = numeric_limits<signed char>::max();
176  A[idx] = nr;
177 
178  // store cps position and mapping of uuid
179  cps.push_back(initializer_list<int>{x,y});
180  uuid_map[c.first] = nr;
181 
182  // count number of cpss
183  nr++;
184  }
185 }
186 
187 void area_division::initialize_map (int r, int c, vector<signed char> src)
188 {
189  // initialization
190  rows = r;
191  cols = c;
192  ob = 0;
193  gridmap = src;
194 }
195 
196 void area_division::assign (vector<valarray<double>> matrix)
197 {
198  BWlist.resize(nr);
199  for (int r=0; r<nr; r++) {
200  BWlist[r].resize(rows*cols);
201  BWlist[r][cps[r][1] * cols + cps[r][0]] = 1;
202  }
203 
204  ArrayOfElements.clear();
205  ArrayOfElements.resize(nr);
206  for (int i=0; i<rows; i++) {
207  for (int j=0; j<cols; j++) {
208  // unknown grid cell (treat as free)
209  if (gridmap[i*cols+j] == -1) {
210  double minV = matrix[0][i*cols+j];
211  int indMin = 0;
212  for (int r=1; r<nr; r++) {
213  if (matrix[r][i*cols+j] < minV) {
214  minV = matrix[r][i*cols+j];
215  indMin = r;
216  }
217  }
218  A[i*cols+j] = indMin;
219  BWlist[indMin][i*cols+j] = 1;
220  ArrayOfElements[indMin]++;
221  }
222 
223  // obstacle
224  else if (gridmap[i*cols+j] < numeric_limits<signed char>::max()) {
225  A[i*cols+j] = nr;
226  }
227  }
228  }
229 }
230 
231 valarray<double> area_division::FinalUpdateOnMetricMatrix(double CM, valarray<double> curentONe, valarray<float> CC)
232 {
233  valarray<double> MMnew(rows*cols);
234 
235  for (int i=0; i<MMnew.size(); ++i) {
236  MMnew[i] = curentONe[i] * CM * CC[i];
237  }
238 
239  return MMnew;
240 }
241 
242 
244 {
245  int maxCellsAss = 0;
246  int minCellsAss = numeric_limits<int>::max();
247 
248 
249  for (int r=0; r<nr; r++) {
250  if (maxCellsAss < ArrayOfElements[r]) {
251  maxCellsAss = ArrayOfElements[r];
252  }
253  if (minCellsAss > ArrayOfElements[r]) {
254  minCellsAss = ArrayOfElements[r];
255  }
256 
257  if (!regions[r]) {
258  return false;
259  }
260  }
261 
262  return (maxCellsAss - minCellsAss) <= thres;
263 }
264 
265 valarray<float> area_division::CalcConnectedMultiplier(valarray<float> dist1, valarray<float> dist2)
266 {
267  valarray<float> returnM(rows*cols);
268  float MaxV = 0;
269  float MinV = numeric_limits<float>::max();
270  for (int i=0;i<rows;i++){
271  for (int j=0;j<cols;j++){
272  returnM[i*cols+j] = dist1[i*cols+j] - dist2[i*cols+j];
273  if (MaxV < returnM[i*cols+j]) {MaxV = returnM[i*cols+j];}
274  if (MinV > returnM[i*cols+j]) {MinV = returnM[i*cols+j];}
275  }
276  }
277 
278  for (int i=0;i<rows;i++){
279  for (int j=0;j<cols;j++){
280  returnM[i*cols+j] =(returnM[i*cols+j] - MinV)*((2*(float)variate_weight)/(MaxV-MinV))+(1-(float)variate_weight);
281  }
282  }
283 
284  return returnM;
285 }
valarray< int > A
Area assignment matrix.
map< string, int > uuid_map
UUID mapping of CPSs.
void assign(vector< valarray< double >> matrix)
Define the assignment matrix based on a given distance metric matrix.
int getMaxLabel()
Get the max label of the labeling process which is in the range [0,max_label].
int max_iter
Maximum number of iterations of the optimization process.
void constructBinaryImages(int robotsLabel)
Create binary arrays of connected labels.
vector< bool > regions
Whether the regions of the robots are connected.
vector< vector< int > > cps
Positions of the CPSs.
int discr
Maximum difference between number of assigned grid cells to each CPS.
bool isThisAGoalState(int thres)
Check if the areas assigned to each robot are of similar size.
Use row-by-row labeling algorithm to label connected components. The algorithm makes two passes over ...
int rows
Number of rows in the grid map.
double variate_weight
Maximum variate weight of connected components.
valarray< float > NormalizedEuclideanDistanceBinary(bool RobotR)
Calculate the normalized euclidean distance transform of a binary image with foreground pixels set to...
void initialize_cps(map< string, vector< int >> cpss)
Define the CPS positions.
void divide()
Perform the area division.
valarray< int > compactLabeling()
Rearrange the labels to make the label numbers consecutive.
vector< valarray< int > > BWlist
A binary array for each CPS which indicates free space.
void initialize_map(int r, int c, vector< signed char > src)
Define the grid map.
vector< int > ArrayOfElements
Grid cells not assigned to the robots.
bool success
Whether the assignment succeeded.
area_division()
Constructor.
int ob
Number of obstacles.
valarray< double > FinalUpdateOnMetricMatrix(double CM, valarray< double > curentONe, valarray< float > CC)
Update the metric matrix.
int cols
Number of columns in the grid map.
nav_msgs::OccupancyGrid get_grid(nav_msgs::OccupancyGrid map, string cps)
Get the region assigned to a CPS.
vector< signed char > gridmap
The environment grid map. Robots are represented by 2, obstacles by 1/-2, empty cells by -1...
valarray< float > CalcConnectedMultiplier(valarray< float > dist1, valarray< float > dist2)
Calculate the connected multiplier.
int nr
Number of robots.


area_division
Author(s): Micha Sende
autogenerated on Thu Oct 31 2019 09:22:46