omt_solver.h
Go to the documentation of this file.
1 #pragma once
2 // std
3 #include <stdio.h>
4 #include <stdlib.h>
5 #include <string.h>
6 #include <vector>
7 #include <list>
8 #include <set>
9 #include <unistd.h> // sleep
10 // eigen
11 #include <Eigen/Dense>
12 // other headers
13 #include "global.h"
14 #include "data_engine.h" // todo: remove.
15 #include "geodesic/Xin_Wang.h"
16 
17 // cluster
18 struct Cluster
19 {
20  int label = -1;
21  Eigen::Vector2d centroid;
22  vector<int> samples;
23  Cluster()
24  {
25  label = -1;
26  centroid = Eigen::Vector2d(0, 0);
27  if (!samples.empty())
28  samples.clear();
29  }
30  Cluster(int l, Eigen::Vector2d p, vector<int> s)
31  {
32  label = l;
33  centroid = Eigen::Vector2d(p.x(), p.y());
34  samples.clear();
35  for (int i = 0; i < s.size(); i++)
36  samples.push_back(s[i]);
37  }
38  Cluster(const Cluster & other)
39  {
40  label = other.label;
41  centroid = Eigen::Vector2d(other.centroid.x(), other.centroid.y());
42  samples.clear();
43  for (int i = 0; i < other.samples.size(); i++)
44  samples.push_back(other.samples[i]);
45  }
46  Cluster & operator = (const Cluster & other)
47  {
48  label = other.label;
49  centroid = Eigen::Vector2d(other.centroid.x(), other.centroid.y());
50  samples.clear();
51  for (int i = 0; i < other.samples.size(); i++)
52  samples.push_back(other.samples[i]);
53  return *this;
54  }
55 };
56 
57 // omt solver
58 class DiscreteSolver
59 {
60  // input: domain, cost metric, sources, targets.
61  // output: assignment from sources to targets.
62 
63  DataEngine* m_p_de; // domain.
64  CDT m_domain; // domain.
65  DistanceMetric* m_metric; // metric.
66  //vector<double>(*CostFunc)(Eigen::Vector2d, vector<Eigen::Vector2d>); // metric. todo: remove.
67  std::vector<Eigen::Vector2d> m_sources; // robot coordinates.
68  std::vector<Eigen::Vector2d> m_targets; // task coordinates.
69  double m_delta; // param delta to constrain the compactness and capacity.
70  std::vector<std::vector<int>> m_result_s_t; // results.
71 public:
72  // constructor.
73  DiscreteSolver(DataEngine& de, CDT d,
74 
75  DistanceMetric& dm,
76 
77  //vector<double>(*cf)(Eigen::Vector2d, vector<Eigen::Vector2d>),
78 
79  vector<Eigen::Vector2d> s, vector<Eigen::Vector2d> t, double delta)
80  {
81  // domain
82  m_p_de = &de;
83  // domain
84  this->m_domain = CDT(d);
85  // metric
86  m_metric = &dm;
87  // metric
88  //CostFunc = cf;
89  // sources
90  this->m_sources.clear();
91  for (int i = 0; i < s.size(); i++)
92  this->m_sources.push_back(s[i]);
93  // targets
94  this->m_targets.clear();
95  for (int i = 0; i < t.size(); i++)
96  this->m_targets.push_back(t[i]);
97  // delta
98  this->m_delta = delta;
99  // results
100  this->m_result_s_t.clear();
101  this->m_result_s_t.resize(this->m_sources.size());
102  // end.
103  return;
104  }
105  // destructor
106  ~DiscreteSolver()
107  {
108  // todo: CDT need manually release?
109  vector<Eigen::Vector2d>().swap(m_sources);
110  vector<Eigen::Vector2d>().swap(m_targets);
111  for (int i = 0; i < m_result_s_t.size(); i++)
112  vector<int>().swap(m_result_s_t[i]);
113  vector<vector<int>>().swap(m_result_s_t);
114  return;
115  }
116 
117  // execute the solveer
118  bool SolveOMT(int mode = 0);
119  // results
120  std::vector<std::vector<int>> getSolution(){ return m_result_s_t; }
121  // visualize domain
122  void CerrDomainCDT();
123  // visualize xxx
124  void CerrClusters(std::vector<Cluster> clusters);
125 
126 //private:
127 
128  // cluster
129  std::vector<Cluster> ClusterTargets(int mode = 0);
130  // match
131  std::vector<int> Match(std::vector<Cluster> clusters);
132 };
133 
134 using namespace std;
135 
136 // hungarian algorithm O(n^3)
137 vector<int> hungarian_algorithm(Eigen::MatrixXd cost)
138 {
139  /* cost matrix format
140 
141  target_1 target_2 ... target_m
142  source_1
143  source_2
144  .
145  .
146  .
147  source_m
148 
149  */
150 
151  // detail info
152  bool log = false;
153 
154  // results
155  vector<int> assignment(cost.rows());
156 
157  // Step 1: Subtract row minima
158 Step1:
159  {
160  for (int r = 0; r < cost.rows(); r++)
161  {
162  double minima = DBL_MAX;
163  for (int c = 0; c < cost.cols(); c++)
164  if (cost(r, c) < minima)
165  minima = cost(r, c);
166  for (int c = 0; c < cost.cols(); c++)
167  cost(r, c) -= minima;
168  }
169  if (log)
170  cerr << "Subtract row minima" << endl << cost << endl << endl;
171  }
172 
173  // Step 2: Subtract column minima
174 Step2:
175  {
176  for (int c = 0; c < cost.cols(); c++)
177  {
178  double minima = DBL_MAX;
179  for (int r = 0; r < cost.rows(); r++)
180  if (cost(r, c) < minima)
181  minima = cost(r, c);
182  for (int r = 0; r < cost.rows(); r++)
183  cost(r, c) -= minima;
184  }
185  if (log)
186  cerr << "Subtract column minima" << endl << cost << endl << endl;
187  }
188 
189  // Step 3: Cover all zeros with a minimum number of lines
190  vector<bool> tick_rows(cost.rows());
191  vector<bool> tick_cols(cost.cols());
192  vector<vector<bool>> circle;
193  vector<vector<bool>> xxxxxx;
194 Step3:
195  circle.clear();
196  xxxxxx.clear();
197  {
198  circle.resize(cost.rows());
199  xxxxxx.resize(cost.rows());
200  for (int r = 0; r < cost.rows(); r++)
201  {
202  circle[r].resize(cost.cols());
203  xxxxxx[r].resize(cost.cols());
204  for (int c = 0; c < cost.cols(); c++)
205  {
206  circle[r][c] = false;
207  xxxxxx[r][c] = false;
208  }
209  }
210  }
211 Check3:
212  tick_rows.clear();
213  tick_rows.resize(cost.rows());
214  tick_cols.clear();
215  tick_cols.resize(cost.cols());
216  {
217  // count num of '0'
218  vector<int> num_rows_0(cost.rows());
219  vector<int> num_cols_0(cost.cols());
220  {
221  for (int r = 0; r < num_rows_0.size(); r++)
222  num_rows_0[r] = 0;
223  for (int c = 0; c < num_cols_0.size(); c++)
224  num_cols_0[c] = 0;
225  for (int r = 0; r < cost.rows(); r++)
226  {
227  for (int c = 0; c < cost.cols(); c++)
228  {
229  if (cost(r, c) < 0.0001)
230  {
231  num_rows_0[r]++;
232  num_cols_0[c]++;
233  }
234  }
235  }
236  }
237 
238  // label '0' by circle or xxxxxx
239  {
240  // labeling
241  int circle_num = 0;
242  int xxxxxx_num = 0;
243  int circle_crt = 0;
244  int xxxxxx_crt = 0;
245  labeling:
246  circle_crt = 0;
247  xxxxxx_crt = 0;
248  for (int r = 0; r < cost.rows(); r++)
249  {
250  int count = 0;
251  int index = -1;
252  for (int c = 0; c < cost.cols(); c++)
253  //if (cost(r, c) < 0.0001)
254  if (cost(r, c) < 0.0001 && !xxxxxx[r][c])
255  {
256  count++;
257  index = c;
258  }
259  if (count == 1)
260  {
261  circle[r][index] = true;
262  for (int tr = 0; tr < cost.rows(); tr++)
263  if (cost(tr, index) < 0.0001 && !circle[tr][index])
264  xxxxxx[tr][index] = true;
265  }
266  }
267  for (int c = 0; c < cost.cols(); c++)
268  {
269  int count = 0;
270  int index = -1;
271  for (int r = 0; r < cost.rows(); r++)
272  if (cost(r, c) < 0.0001 && !xxxxxx[r][c])
273  {
274  count++;
275  index = r;
276  }
277  if (count == 1)
278  {
279  circle[index][c] = true;
280  for (int tc = 0; tc < cost.cols(); tc++)
281  if (cost(index, tc) < 0.0001 && !circle[index][tc])
282  xxxxxx[index][tc] = true;
283  }
284  }
285  // update crt
286  for (int r = 0; r < cost.rows(); r++)
287  {
288  for (int c = 0; c < cost.cols(); c++)
289  {
290  if (circle[r][c])
291  circle_crt++;
292  if (xxxxxx[r][c])
293  xxxxxx_crt++;
294  }
295  }
296  // crt vs. num
297  if (circle_crt == circle_num && xxxxxx_crt == xxxxxx_num)
298  {
299 
300  }
301  else
302  {
303  circle_num = circle_crt;
304  xxxxxx_num = xxxxxx_crt;
305  goto labeling;
306  }
307  }
308  // check if left '0' unlabeled
309  Check:
310  {
311  for (int r = 0; r < cost.rows(); r++)
312  {
313  for (int c = 0; c < cost.cols(); c++)
314  {
315  if (!circle[r][c] && !xxxxxx[r][c] && cost(r, c) < 0.0001)
316  {
317  circle[r][c] = true;
318  for (int tr = 0; tr < cost.rows(); tr++)
319  {
320  if (cost(tr, c) < 0.0001
321  && !circle[tr][c] && !xxxxxx[tr][c])
322  {
323  xxxxxx[tr][c] = true;
324  }
325  }
326  for (int tc = 0; tc < cost.cols(); tc++)
327  {
328  if (cost(r, tc) < 0.0001
329  && !circle[r][tc] && !xxxxxx[r][tc])
330  {
331  xxxxxx[r][tc] = true;
332  }
333  }
334  goto Check3;
335  }
336  }
337  }
338  }
339 
340  // line cover
341  {
342  // tick
343  {
344  // init
345  for (int r = 0; r < tick_rows.size(); r++)
346  tick_rows[r] = false;
347  for (int c = 0; c < tick_cols.size(); c++)
348  tick_cols[c] = false;
349  // tick step 1
350  for (int r = 0; r < cost.rows(); r++)
351  {
352  bool exist = false;
353  for (int c = 0; c < cost.cols(); c++)
354  if (circle[r][c])
355  exist = true;
356  if (!exist)
357  {
358  tick_rows[r] = true;
359  }
360  }
361  // loop
362  int tick_num = 0;
363  for (int r = 0; r < tick_rows.size(); r++)
364  if (tick_rows[r])
365  tick_num++;
366  for (int c = 0; c < tick_cols.size(); c++)
367  if (tick_cols[c])
368  tick_num++;
369  int crt_num = 0;
370  while (crt_num != tick_num)
371  {
372  tick_num = crt_num;
373  // tick step 2
374  for (int r = 0; r < cost.rows(); r++)
375  {
376  if (tick_rows[r])
377  {
378  for (int c = 0; c < cost.cols(); c++)
379  {
380  if (cost(r, c) < 0.0001 && xxxxxx[r][c])
381  {
382  tick_cols[c] = true;
383  }
384  }
385  }
386  }
387  // tick step 3
388  for (int c = 0; c < cost.cols(); c++)
389  {
390  if (tick_cols[c])
391  {
392  for (int r = 0; r < cost.rows(); r++)
393  {
394  if (cost(r, c) < 0.0001 && circle[r][c])
395  {
396  tick_rows[r] = true;
397  }
398  }
399  }
400  }
401  // update crt_num
402  crt_num = 0;
403  for (int r = 0; r < tick_rows.size(); r++)
404  if (tick_rows[r])
405  crt_num++;
406  for (int c = 0; c < tick_cols.size(); c++)
407  if (tick_cols[c])
408  crt_num++;
409  }
410  }
411 
412  // check
413  if (log)
414  {
415  Eigen::MatrixXd lmat(cost.rows(), cost.cols());
416  for (int r = 0; r < lmat.rows(); r++)
417  for (int c = 0; c < lmat.cols(); c++)
418  lmat(r, c) = 0;
419  for (int r = 0; r < cost.rows(); r++)
420  if (!tick_rows[r])
421  for (int c = 0; c < cost.cols(); c++)
422  lmat(r, c) = 1;
423  for (int c = 0; c < cost.cols(); c++)
424  if (tick_cols[c])
425  for (int r = 0; r < cost.rows(); r++)
426  lmat(r, c) = 1;
427  cerr << "Cover all zeros with a minimum number of lines" << endl
428  << lmat << endl << endl;
429  }
430 
431  // check number of line cover
432  {
433  int line_num = 0;
434  for (int r = 0; r < cost.rows(); r++)
435  if (!tick_rows[r])
436  {
437  line_num++;
438  }
439  for (int c = 0; c < cost.cols(); c++)
440  if (tick_cols[c])
441  {
442  line_num++;
443  }
444 
445  if (line_num == cost.rows())
446  goto Final;
447  else
448  goto Step4;
449  }
450  }
451  }
452 
453  // Step 4: Create additional zeros
454 Step4:
455  {
456  // line covered
457  vector<vector<bool>> covered;
458  // init
459  covered.resize(cost.rows());
460  for (int r = 0; r < cost.rows(); r++)
461  {
462  covered[r].resize(cost.cols());
463  for (int c = 0; c < cost.cols(); c++)
464  {
465  covered[r][c] = false;
466  }
467  }
468  // check number of line cover
469  {
470  for (int r = 0; r < cost.rows(); r++)
471  if (!tick_rows[r])
472  {
473  // line cover
474  for (int c = 0; c < cost.cols(); c++)
475  {
476  covered[r][c] = true;
477  }
478  }
479  for (int c = 0; c < cost.cols(); c++)
480  if (tick_cols[c])
481  {
482  // line cover
483  for (int r = 0; r < cost.rows(); r++)
484  {
485  covered[r][c] = true;
486  }
487  }
488 
489  }
490  // minima of uncovered
491  {
492  double minima = DBL_MAX;
493  for (int r = 0; r < cost.rows(); r++)
494  {
495  for (int c = 0; c < cost.cols(); c++)
496  {
497  if (!covered[r][c])
498  {
499  if (cost(r, c) < minima)
500  {
501  minima = cost(r, c);
502  }
503  }
504  }
505  }
506 
507  // plus and minus
508  {
509  for (int r = 0; r < cost.rows(); r++)
510  {
511  for (int c = 0; c < cost.cols(); c++)
512  {
513  if (!covered[r][c])
514  {
515  cost(r, c) -= minima;
516  }
517  else
518  {
519  if (!tick_rows[r] && tick_cols[c])
520  cost(r, c) += minima;
521  }
522  }
523  }
524  }
525  }
526 
527  if (log)
528  cerr << "Create additional zeros" << endl << cost << endl << endl;
529 
530  goto Step3;
531  }
532 
533  // final step
534 Final:
535  {
536  // save assignment
537  for (int r = 0; r < cost.rows(); r++)
538  {
539  for (int c = 0; c < cost.cols(); c++)
540  {
541  if (circle[r][c])
542  {
543  assignment[r] = c;
544  }
545  }
546  }
547  // log
548  if (log)
549  {
550  Eigen::MatrixXd opt(cost.rows(), cost.cols());
551  for (int r = 0; r < opt.rows(); r++)
552  for (int c = 0; c < opt.cols(); c++)
553  opt(r, c) = 0;
554  for (int r = 0; r < cost.rows(); r++)
555  for (int c = 0; c < cost.cols(); c++)
556  if (circle[r][c])
557  opt(r, c) = 1;
558  cerr << "result" << endl << opt << endl << endl;
559  }
560  }
561 
562  return assignment;
563 }
564 
565 // visualize cdt
566 void DiscreteSolver::CerrDomainCDT()
567 {
568  // cdt
569  for (auto it = m_domain.finite_faces_begin(); it != m_domain.finite_faces_end(); it++)
570  {
571  if (it->info().in_domain())
572  {
574  //cerr << "plt.plot([" << it->vertex(0)->point().x() << ", " << it->vertex(1)->point().x() << "], ["
575  // << it->vertex(0)->point().y() << ", " << it->vertex(1)->point().y() << "], 'k-')" << endl;
576  //cerr << "plt.plot([" << it->vertex(1)->point().x() << ", " << it->vertex(2)->point().x() << "], ["
577  // << it->vertex(1)->point().y() << ", " << it->vertex(2)->point().y() << "], 'k-')" << endl;
578  //cerr << "plt.plot([" << it->vertex(2)->point().x() << ", " << it->vertex(0)->point().x() << "], ["
579  // << it->vertex(2)->point().y() << ", " << it->vertex(0)->point().y() << "], 'k-')" << endl;
580 
581  //
582  cerr << "plt.fill([" << it->vertex(0)->point().x() << ", " << it->vertex(1)->point().x() << ", " << it->vertex(2)->point().x()
583  << "], [" << it->vertex(0)->point().y() << ", " << it->vertex(1)->point().y() << ", " << it->vertex(2)->point().y()
584  << "], color=[0.7, 0.7, 0.7])" << endl;
585  }
586  }
587  // end
588  return;
589 }
590 
591 // visualize clusters
592 void DiscreteSolver::CerrClusters(vector<Cluster> clusters)
593 {
594  for (int cid = 0; cid < clusters.size(); cid++)
595  {
596  double r = (double)rand() / RAND_MAX;
597  double g = (double)rand() / RAND_MAX;
598  double b = (double)rand() / RAND_MAX;
599  // samples
600  for (int sid = 0; sid < clusters[cid].samples.size(); sid++)
601  {
602  int tid = clusters[cid].samples[sid];
603  char sentence[200];
604  sprintf(sentence, "plt.plot(%f, %f, color=[%f, %f, %f])",
605  m_targets[tid].x(), m_targets[tid].y(), r, g, b);
606  cerr << sentence << endl;
607  }
608  // centroid
609  char sentence[200];
610  sprintf(sentence, "plt.plot(%f, %f, marker='x', color=[%f, %f, %f])",
611  clusters[cid].centroid.x(), clusters[cid].centroid.y(), r, g, b);
612  cerr << sentence << endl;
613  }
614  return;
615 }
616 
617 // cluster targets
618 vector<Cluster> DiscreteSolver::ClusterTargets(int mode)
619 {
620  // timing
621  double t_beg = clock();
622 
623  // set up params
624  int max_iter_times = 10;
625 
626  // initialization
627  vector<Cluster> clusters;
628  clusters.resize(m_sources.size());
629  for (int idx = 0; idx < clusters.size(); idx++)
630  clusters[idx].centroid = m_sources[idx];
631 
632  // iteration begin
633  for (int it = 0; it < max_iter_times; it++)
634  {
635  cerr << "cluster iteration = " << it << endl;
636 
637  // record last centroid
638  vector<Eigen::Vector2d> last_centroids(clusters.size());
639  for (int cid = 0; cid < clusters.size(); cid++)
640  last_centroids[cid] = Eigen::Vector2d(clusters[cid].centroid.x(), clusters[cid].centroid.y());
641 
642  // clear samples
643  for (int cid = 0; cid < clusters.size(); cid++)
644  clusters[cid].samples.clear();
645 
646  // compute label for each sample
647  {
648  // memory alloc
649  int dRange = max_iter_times * m_targets.size();
650  double ** m_tid_cid_d = new double*[dRange];
651  while (m_tid_cid_d == 0)
652  {
653  cerr << "memory error, re assign memory..." << endl;
654  sleep(0.1);
655  m_tid_cid_d = new double*[dRange];
656  }
657  for (int tid = 0; tid < dRange; tid++)
658  {
659  m_tid_cid_d[tid] = new double[dRange];
660  while (m_tid_cid_d[tid] == 0)
661  {
662  cerr << "memory error, re assign memory..." << endl;
663  sleep(0.1);
664  m_tid_cid_d[tid] = new double[dRange];
665  }
666  }
667  // compute distance
668 #pragma omp parallel for num_threads(omp_get_num_procs())
669  for (int cid = 0; cid < clusters.size(); cid++)
670  {
671  //vector<double> cDistances = CostFunc(clusters[cid].centroid, m_targets); // compute distance
672  vector<double> cDistances = m_metric->GeodesicDistances(clusters[cid].centroid, m_targets); // compute distance
673  for (int tid = 0; tid < m_targets.size(); tid++)
674  {
675  if (cid >= dRange)
676  {
677  char str[200];
678  sprintf(str, "cid = %d, dRange = %d", cid, dRange);
679  cout << str << endl;
680  cout << "error: cid out bound" << endl;
681  }
682  if (tid >= dRange)
683  {
684  char str[200];
685  sprintf(str, "tid = %d, dRange = %d", tid, dRange);
686  cout << str << endl;
687  cout << "error: tid out bound" << endl;
688  }
689  m_tid_cid_d[tid][cid] = cDistances[tid];
690  }
691  }
692  // compute label
693  for (int tid = 0; tid < m_targets.size(); tid++)
694  {
695  int c = 0;
696  double min_distance = DBL_MAX;
697  for (int cid = 0; cid < clusters.size(); cid++)
698  {
699  double distance = m_tid_cid_d[tid][cid];
700  if (distance < min_distance)
701  {
702  min_distance = distance;
703  c = cid;
704  }
705  }
706  clusters[c].samples.push_back(tid);
707  }
708  // memory release
709  for (int tid = 0; tid < dRange; tid++)
710  delete[] m_tid_cid_d[tid];
711  delete[] m_tid_cid_d;
712  }
713 
714  // compute centroids
715 #pragma omp parallel for num_threads(omp_get_num_procs())
716  for (int cid = 0; cid < clusters.size(); cid++)
717  {
718  // check empty
719  if (clusters[cid].samples.empty())
720  {
721  clusters[cid].centroid = Eigen::Vector2d(999999, 999999);
722  continue;
723  }
724  // compute centroid.
725  Eigen::Vector2d val(0, 0);
726  for (int sid = 0; sid < clusters[cid].samples.size(); sid++)
727  {
728  int sample = clusters[cid].samples[sid];
729  val = Eigen::Vector2d(val.x() + m_targets[sample].x(), val.y() + m_targets[sample].y());
730  }
731  clusters[cid].centroid = Eigen::Vector2d(val.x() / clusters[cid].samples.size(), val.y() / clusters[cid].samples.size());
732  // inner centroid. cellmap?
733  {
734  int r = (int)round(-clusters[cid].centroid.y());
735  int c = (int)round(clusters[cid].centroid.x());
736  // check out bound
737  if (m_p_de->m_recon2D.m_cellmap[r][c].isScanned && m_p_de->m_recon2D.m_cellmap[r][c].isFree)
738  {
739  ; // nothing to do here.
740  }
741  else
742  {
743  // replease with the closest point in domain
744  double min_d = DBL_MAX;
745  Eigen::Vector2d min_p;
746  for (auto it = m_domain.vertices_begin(); it != m_domain.vertices_end(); it++)
747  {
748  Eigen::Vector2d p(it->point().x(), it->point().y());
749  double d = (clusters[cid].centroid - p).norm();
750  if (d < min_d)
751  {
752  min_d = d;
753  min_p = Eigen::Vector2d(p);
754  }
755  }
756  clusters[cid].centroid = min_p;
757  }
758  }
759  }
760 
761  // split (compactness & capacity)
762  for (int cid = 0; cid < clusters.size(); cid++) // for each cluster, check innner distance
763  {
764  if (clusters[cid].samples.size() <= 1) continue;
765  // compute iner distances
766  vector<Eigen::Vector2d> targets;
767  for (int tid = 0; tid < clusters[cid].samples.size(); tid++)
768  targets.push_back(Eigen::Vector2d(m_targets[clusters[cid].samples[tid]].x(), m_targets[clusters[cid].samples[tid]].y()));
769  //vector<double> inDistances = CostFunc(clusters[cid].centroid, targets);
770  vector<double> inDistances = m_metric->GeodesicDistances(clusters[cid].centroid, targets);
771  // max distance element
772  int maxID = -1;
773  double maxDistance = -1;
774  for (int i = 0; i < inDistances.size(); i++)
775  {
776  if (maxDistance < inDistances[i])
777  {
778  maxDistance = inDistances[i];
779  maxID = i;
780  }
781  }
782  if (maxID == -1)
783  {
784  cerr << "error: maxID == -1." << endl;
785  getchar(); getchar(); getchar();
786  }
787  if (maxID >= clusters[cid].samples.size() || maxID >= m_targets.size())
788  {
789  cerr << "error: maxID out bound." << endl;
790  cerr << "maxID = " << maxID << endl;
791  cerr << "cluster samples num = " << clusters[cid].samples.size() << endl;
792  cerr << "targets num = " << m_targets.size() << endl;
793  getchar(); getchar(); getchar();
794  }
795  // if need split
796  if (maxDistance > compactParam)
797  {
798  // insert new cluster
799  Cluster new_cluster;
800  new_cluster.label = -1; // todo: id.
801  new_cluster.samples.push_back(clusters[cid].samples[maxID]);
802  new_cluster.centroid = Eigen::Vector2d(m_targets[clusters[cid].samples[maxID]].x(), m_targets[clusters[cid].samples[maxID]].y());
803  clusters.push_back(new_cluster);
804  // erase the sample from origin cluster
805  clusters[cid].samples.erase(clusters[cid].samples.begin() + maxID);
806  }
807  }
808 
809  // merge
810  int valid_cluster_num = 0;
811  for (int cid = 0; cid < clusters.size(); cid++)
812  {
813  if (clusters[cid].samples.empty())
814  continue;
815  valid_cluster_num++;
816  }
817  if (valid_cluster_num <= m_sources.size()) // useless?
818  {
819  }
820  //else if (!allowFreeRobot) // useless?
821  //{
822  //}
823  else
824  {
825  bool nMerge = false;
826  for (int cid = 0; cid < clusters.size() - 1; cid++)
827  {
828  if (clusters[cid].samples.empty()) continue;
829  for (int oid = cid + 1; oid < clusters.size(); oid++)
830  {
831  if (clusters[oid].samples.empty()) continue;
832  // compute distance
833  Point_2 p1(clusters[cid].centroid.x(), clusters[cid].centroid.y());
834  Point_2 p2(clusters[oid].centroid.x(), clusters[oid].centroid.y());
835  double outerD = m_metric->get_geodesic_distance_fast(p1, p2); // todo: change it to func pointer.
836  if (outerD < compactParam) // need to merge
837  {
838  if (clusters[oid].samples.size() == 1) // todo: .
839  {
840  clusters[cid].samples.push_back(clusters[oid].samples[0]);
841  clusters[oid].samples.clear();
842  nMerge = true;
843  }
844  else if (clusters[cid].samples.size() == 1)
845  {
846  clusters[oid].samples.push_back(clusters[cid].samples[0]);
847  clusters[cid].samples.clear();
848  nMerge = true;
849  }
850  // merge done.
851  valid_cluster_num--;
853  //if (validNumber <= rbtPositions.size())
854  // break;
855  }
856  }
857  }
858  }
859 
860  // delete empty clusters
861  {
862  vector<Cluster> temp_clusters;
863  for (int cid = 0; cid < clusters.size(); cid++)
864  {
865  if (!clusters[cid].samples.empty())
866  {
867  temp_clusters.push_back(Cluster(clusters[cid]));
868  }
869  }
870  vector<Cluster>().swap(clusters);
871  clusters = temp_clusters;
872  }
873 
874  // update centroids
875  {
876  for (int cid = 0; cid < clusters.size(); cid++)
877  {
878  if (clusters[cid].samples.empty())
879  {
880  clusters[cid].centroid = Eigen::Vector2d(999999, 999999);
881  continue;
882  }
883  // update centroid
884  Eigen::Vector2d val(0, 0);
885  for (int sid = 0; sid < clusters[cid].samples.size(); sid++)
886  {
887  int sample = clusters[cid].samples[sid];
888  val = Eigen::Vector2d(val.x() + m_targets[sample].x(), val.y() + m_targets[sample].y());
889  }
890  clusters[cid].centroid = Eigen::Vector2d(val.x() / clusters[cid].samples.size(), val.y() / clusters[cid].samples.size());
891  // inner centroid. cellmap?
892  {
893  int r = (int)round(-clusters[cid].centroid.y());
894  int c = (int)round(clusters[cid].centroid.x());
895  // check out bound
896  if (m_p_de->m_recon2D.m_cellmap[r][c].isScanned && m_p_de->m_recon2D.m_cellmap[r][c].isFree)
897  {
898  ; // nothing to do here.
899  }
900  else
901  {
902  // replease with the closest point in domain
903  double min_d = DBL_MAX;
904  Eigen::Vector2d min_p;
905  for (auto it = m_domain.vertices_begin(); it != m_domain.vertices_end(); it++)
906  {
907  Eigen::Vector2d p(it->point().x(), it->point().y());
908  double d = (clusters[cid].centroid - p).norm();
909  if (d < min_d)
910  {
911  min_d = d;
912  min_p = Eigen::Vector2d(p);
913  }
914  }
915  clusters[cid].centroid = min_p;
916  }
917  }
918  }
919  }
920 
921  // iteration difference
922  {
923  bool terminate = false;
924  if (last_centroids.size() == clusters.size())
925  {
926  double diff = 0;
927  for (int cid = 0; cid < clusters.size(); cid++)
928  {
929  diff += (last_centroids[cid] - clusters[cid].centroid).norm(); // todo: func pointer.
930  if (diff > 0.001)
931  {
932  break;
933  }
934  }
935  if (diff < 0.001) terminate = true;
936  }
937  // terminate
938  if (terminate)
939  {
940  cerr << "converged at iter " << it << "." << endl;
941  break;
942  }
943  }
944 
945  // if (!allowFreeRobot) // avoid #cluster_not_empty < #robot
946  // {
947  // // compute valid cluster number
948  // int cnt = 0;
949  // for (int rid = 0; rid < regions.size(); rid++)
950  // {
951  // if (regions[rid].samples.empty())
952  // continue;
953  // cnt++;
954  // }
955  // while (cnt < rbtPositions.size() && tasks.size() >= rbtPositions.size()) // if #cluster_not_empty < #robot, split a cluster.
956  // {
957  // //cerr << "robot free of tasks, finding split..." << endl;
958  // //cout << "robot free of tasks, finding split..." << endl;
959  // //cout << "#rbt = " << rbtPositions.size() << ", #cluster = " << cnt << endl;
960  // // find the selected region: farthest iner distance
961  // double farInerDistance = -1;
962  // int selectedRid = -1;
963  // int selectedSid = -1;
964  // for (int orid = 0; orid < regions.size(); orid++)
965  // {
966  // if (regions[orid].samples.size() <= 1)
967  // continue;
968  // for (int sid = 0; sid < regions[orid].samples.size(); sid++)
969  // {
970  // int tid = regions[orid].samples[sid];
971  // // compare option 2: distance from centroid. 2018-10-22.
972  // //double distance = m_metric->get_geodesic_distance(regions[orid].centroid, Point_2(tasks[tid].pose.translation().x(), tasks[tid].pose.translation().y()));
973  // double distance = m_metric->get_geodesic_distance_fast(regions[orid].centroid, Point_2(tasks[tid].pose.translation().x(), tasks[tid].pose.translation().y()));
974  // if (distance > farInerDistance)
975  // {
976  // farInerDistance = distance;
977  // selectedRid = orid;
978  // selectedSid = sid;
979  // }
980  // }
981  // }
982  // // no candidate
983  // if (selectedRid == -1 || selectedSid == -1)
984  // {
985  // break;
986  // }
987  //
988  // // insert the sample
989  // Region nRegion;
990  // nRegion.label = -1;
991  // nRegion.samples.push_back(regions[selectedRid].samples[selectedSid]);
992  // nRegion.centroid = Point_2(tasks[regions[selectedRid].samples[selectedSid]].pose.translation().x(), tasks[regions[selectedRid].samples[selectedSid]].pose.translation().y());
993  // regions.push_back(nRegion);
994  // cnt++; // update count.
995  //
996  // // remove the sample
997  // regions[selectedRid].samples.erase(regions[selectedRid].samples.begin() + selectedSid);
998  //
999  // //cerr << "split cluster because of free robot." << endl;
1000  // //cout << "split cluster because of free robot." << endl;
1001  // //cout << "current state: " << endl;
1002  //
1003  // int tmpCnt = 0;
1004  // for (int tmpID = 0; tmpID < regions.size(); tmpID++)
1005  // {
1006  // if (regions[tmpID].samples.empty())
1007  // continue;
1008  // //cout << "region_" << tmpID << " sample_0 " << tasks[regions[tmpID].samples[0]].pose.translation().transpose() << endl;
1009  // //cout << "region_" << tmpID << " centroid " << regions[tmpID].centroid.x() << " " << regions[tmpID].centroid.y() << endl;
1010  // tmpCnt++;
1011  // }
1012  // //cout << tmpCnt << " valid clusters." << endl << endl;
1013  // }
1014  // }
1015  //
1016  // // difference of centroids
1017  // double crtResCentroids = 0;
1018  //
1019  // /* update centroid */ // update 2018-11-14.
1020  // for (int rid = 0; rid < regions.size(); rid++)
1021  // {
1022  // if (regions[rid].samples.empty())
1023  // {
1024  // regions[rid].centroid = Point_2(999999, 999999);
1025  // continue;
1026  // }
1027  // // update centroid
1028  // Point_2 val(0, 0);
1029  // for (int sid = 0; sid < regions[rid].samples.size(); sid++)
1030  // {
1031  // int sample = regions[rid].samples[sid];
1032  // val = Point_2(val.x() + tasks[sample].pose.translation().x(), val.y() + tasks[sample].pose.translation().y());
1033  // }
1034  // regions[rid].centroid = Point_2(val.x() / regions[rid].samples.size(), val.y() / regions[rid].samples.size());
1035  // crtResCentroids += get_euclidean_distance(lastCentroids[rid], regions[rid].centroid);
1036  // }
1037  //
1038  // ///* test plot */
1039  // //{
1040  // // char file_dir[1000];
1041  // // sprintf(file_dir, "data/OfflineOutput/clusters_%d.m", it);
1042  // // ofstream ofs(file_dir);
1043  // // vector<vector<double>> color(regions.size());
1044  // // for (int i = 0; i < color.size(); i++)
1045  // // {
1046  // // color[i].resize(3);
1047  // // for (int j = 0; j < color[i].size(); j++)
1048  // // {
1049  // // color[i][j] = (double)rand() / RAND_MAX;
1050  // // }
1051  // // }
1052  // // for (int cid = 0; cid < regions.size(); cid++)
1053  // // {
1054  // // for (int sid = 0; sid < regions[cid].samples.size(); sid++)
1055  // // {
1056  // // char plotCode[200];
1057  // // sprintf(plotCode, "plot([%f, %f], [%f, %f], 'Color', [%f, %f, %f]); hold on;",
1058  // // regions[cid].centroid.x(), tasks[regions[cid].samples[sid]].pose.translation().x(),
1059  // // regions[cid].centroid.y(), tasks[regions[cid].samples[sid]].pose.translation().y(),
1060  // // color[cid][0], color[cid][1], color[cid][2]);
1061  // // ofs << plotCode << endl;
1062  // // }
1063  // // }
1064  // // ofs.close();
1065  // //}
1066  //
1067  // /* res */
1068  // if (crtResCentroids < 0.001)
1069  // {
1070  // cout << "converged at iter " << it << "." << endl;
1071  // break;
1072  // }
1073 
1074  }
1075 
1076  // delete empty clusters
1077  {
1078  vector<Cluster> temp_clusters;
1079  for (int cid = 0; cid < clusters.size(); cid++)
1080  {
1081  if (!clusters[cid].samples.empty())
1082  {
1083  temp_clusters.push_back(Cluster(clusters[cid]));
1084  }
1085  }
1086  vector<Cluster>().swap(clusters);
1087  clusters = temp_clusters;
1088  }
1089 
1090  // timing
1091  double t_end = clock();
1092  cerr << "DiscreteSolver cluster timing " << (t_end - t_beg)/CLOCKS_PER_SEC << " s" << endl;
1093 
1095  //{
1096  // // seed points
1097  // for (int sid = 0; sid < m_sources.size(); sid++)
1098  // {
1099  // //char color[100];
1100  // //sprintf(color, "[%f, %f, %f]", (double)rand() / RAND_MAX, (double)rand() / RAND_MAX, (double)rand() / RAND_MAX);
1101  // char sentence[100];
1102  // //sprintf(sentence, "plt.plot(%f, %f, color=%s, marker='x')", m_sources[sid].x(), m_sources[sid].y(), color);
1103  // sprintf(sentence, "plt.plot(%f, %f, color=[1, 0, 0], marker='x')", m_sources[sid].x(), m_sources[sid].y());
1104  // cerr << sentence << endl;
1105  // }
1106  // // clusters
1107  // for (int cid = 0; cid < clusters.size(); cid++)
1108  // {
1109  // char color[100];
1110  // sprintf(color, "[%f, %f, %f]", (double)rand() / RAND_MAX, (double)rand() / RAND_MAX, (double)rand() / RAND_MAX);
1111  // for (int sid = 0; sid < clusters[cid].samples.size(); sid++)
1112  // {
1113  // int id = clusters[cid].samples[sid];
1114  // char sentence[100];
1115  // sprintf(sentence, "plt.plot(%f, %f, color=%s, marker='.')", m_targets[id].x(), m_targets[id].y(), color);
1116  // cerr << sentence << endl;
1117  // }
1118  // char sentence[100];
1119  // sprintf(sentence, "plt.plot(%f, %f, color=%s, marker='+')", clusters[cid].centroid.x(), clusters[cid].centroid.y(), color);
1120  // cerr << sentence << endl;
1121  // }
1122  //}
1123 
1124  // end.
1125  return clusters;
1126 }
1127 
1128 // match
1129 vector<int> DiscreteSolver::Match(vector<Cluster> clusters)
1130 {
1132  //{
1133  // //// example#1
1134  // //Eigen::MatrixXd cost(4, 4);
1135  // //cost << 82, 83, 69, 92,
1136  // // 77, 37, 49, 92,
1137  // // 11, 69, 5, 86,
1138  // // 8, 9, 98, 23;
1139  // //// example#2
1140  // //Eigen::MatrixXd cost(5, 5);
1141  // //cost << 5, 0, 2, 0, 2,
1142  // // 2, 3, 0, 0, 0,
1143  // // 0, 10, 5, 7, 2,
1144  // // 9, 8, 0, 0, 4,
1145  // // 0, 6, 3, 6, 5;
1146  // // example#3
1147  // Eigen::MatrixXd cost(10, 10);
1148  // cost << 7, 54, 42, 4, 84, 24, 53, 80, 61, 14,
1149  // 43, 30, 24, 65, 95, 9, 1, 87, 24, 28,
1150  // 63, 23, 61, 68, 33, 37, 53, 45, 80, 44,
1151  // 78, 99, 4, 9, 81, 65, 37, 92, 98, 85,
1152  // 61, 15, 82, 85, 89, 11, 2, 58, 5, 24,
1153  // 41, 39, 29, 45, 55, 19, 36, 95, 31, 51,
1154  // 31, 98, 73, 19, 18, 66, 65, 23, 40, 8,
1155  // 56, 37, 10, 49, 56, 64, 29, 21, 45, 90,
1156  // 15, 81, 24, 33, 76, 3, 44, 54, 97, 70,
1157  // 83, 4, 85, 25, 15, 9, 4, 83, 58, 94;
1158  // // test the case
1159  // cerr << "cost" << endl << cost << endl;
1160  // hungarian_algorithm(cost);
1161  //}
1162 
1163  // init
1164  vector<Eigen::Vector2d> centroids(clusters.size());
1165  for (int cid = 0; cid < clusters.size(); cid++)
1166  centroids[cid] = clusters[cid].centroid;
1167  vector<int> matches(m_sources.size());
1168  for (int sid = 0; sid < m_sources.size(); sid++)
1169  matches[sid] = sid;
1170 
1171  // compute distances
1172  vector<vector<double>> d_s_c(m_sources.size());
1173  for (int s = 0; s < d_s_c.size(); s++)
1174  //d_s_c[s] = CostFunc(m_sources[s], centroids);
1175  d_s_c[s] = m_metric->GeodesicDistances(m_sources[s], centroids);
1176 
1177  // tune
1178  {
1179  // replaced by hungarian.
1180  }
1181 
1182  // match
1183  int dim = max(m_sources.size(), centroids.size());
1184  Eigen::MatrixXd cost(dim, dim);
1185  if (m_sources.size() < centroids.size())
1186  {
1187  for (int r = 0; r < dim; r++)
1188  for (int c = 0; c < dim; c++)
1189  {
1190  if (r >= m_sources.size())
1191  cost(r, c) = 0.0;
1192  else
1193  cost(r, c) = d_s_c[r][c];
1194  }
1195 
1196  // match by hungarian algorithm
1197  //cerr << "cost" << endl << cost << endl;
1198  vector<int> assignment = hungarian_algorithm(cost);
1199  // final match
1200  for (int r = 0; r < m_sources.size(); r++)
1201  matches[r] = assignment[r];
1202  }
1203  else if (m_sources.size() == centroids.size())
1204  {
1205  for (int r = 0; r < dim; r++)
1206  for (int c = 0; c < dim; c++)
1207  cost(r, c) = d_s_c[r][c];
1208 
1209  // match by hungarian algorithm
1210  //cerr << "cost" << endl << cost << endl;
1211  matches = hungarian_algorithm(cost);
1212  }
1213  else
1214  {
1215  for (int r = 0; r < dim; r++)
1216  for (int c = 0; c < dim; c++)
1217  {
1218  if (c >= centroids.size())
1219  cost(r, c) = 0.0;
1220  else
1221  cost(r, c) = d_s_c[r][c];
1222  }
1223 
1224  // match by hungarian algorithm
1225  //cerr << "cost" << endl << cost << endl;
1226  vector<int> assignment = hungarian_algorithm(cost);
1227  // final match
1228  for (int r = 0; r < m_sources.size(); r++)
1229  {
1230  if (assignment[r] >= centroids.size())
1231  matches[r] = -1;
1232  else
1233  matches[r] = assignment[r];
1234  }
1235  }
1236 
1238  //{
1239  // for (int sid = 0; sid < matches.size(); sid++)
1240  // {
1241  // cerr << "source " << sid << " assigned target " << matches[sid] << endl;
1242  // }
1243  // //cerr << "con?" << endl;
1244  // //getchar();
1245  //}
1246 
1247  // end.
1248  return matches;
1249 }
1250 
1251 // solve
1252 bool DiscreteSolver::SolveOMT(int mode)
1253 {
1254  // reset
1255  if (!m_result_s_t.empty())
1256  {
1257  for (int i = 0; i < m_result_s_t.size(); i++)
1258  if (!m_result_s_t[i].empty()) vector<int>().swap(m_result_s_t[i]);
1259  vector<vector<int>>().swap(m_result_s_t);
1260  }
1261  m_result_s_t.resize(m_sources.size());
1262 
1263  // lloyd for term 1&3
1264  vector<Cluster> clusters = ClusterTargets(mode);
1265 
1266  // match for term 2
1267  vector<int> matches = Match(clusters);
1268 
1269  // final assignment
1270  for (int sid = 0; sid < m_sources.size(); sid++)
1271  {
1272  int cid = matches[sid];
1273  if (cid == -1)
1274  continue;
1275  for (int idx = 0; idx < clusters[cid].samples.size(); idx++)
1276  {
1277  int tid = clusters[cid].samples[idx];
1278  m_result_s_t[sid].push_back(tid);
1279  }
1280  }
1281 
1282  //end.
1283  return true;
1284 }
d
d
Point_2
K::Point_2 Point_2
Definition: global.h:32
DiscreteSolver::m_sources
std::vector< Eigen::Vector2d > m_sources
Definition: omt_solver.h:67
data_engine.h
DiscreteSolver::CerrDomainCDT
void CerrDomainCDT()
Definition: omt_solver.h:566
s
XmlRpcServer s
Cluster::operator=
Cluster & operator=(const Cluster &other)
Definition: omt_solver.h:46
hungarian_algorithm
vector< int > hungarian_algorithm(Eigen::MatrixXd cost)
Definition: omt_solver.h:137
DiscreteSolver::m_p_de
DataEngine * m_p_de
Definition: omt_solver.h:63
Cluster::samples
vector< int > samples
Definition: omt_solver.h:22
DiscreteSolver::getSolution
std::vector< std::vector< int > > getSolution()
Definition: omt_solver.h:120
log
EIGEN_DEVICE_FUNC const LogReturnType log() const
Definition: ArrayCwiseUnaryOps.h:105
Xin_Wang.h
DiscreteSolver::m_metric
DistanceMetric * m_metric
Definition: omt_solver.h:65
DiscreteSolver::m_result_s_t
std::vector< std::vector< int > > m_result_s_t
Definition: omt_solver.h:70
DiscreteSolver::ClusterTargets
std::vector< Cluster > ClusterTargets(int mode=0)
Definition: omt_solver.h:618
DiscreteSolver::CerrClusters
void CerrClusters(std::vector< Cluster > clusters)
Definition: omt_solver.h:592
DiscreteSolver::m_targets
std::vector< Eigen::Vector2d > m_targets
Definition: omt_solver.h:68
DiscreteSolver::Match
std::vector< int > Match(std::vector< Cluster > clusters)
Definition: omt_solver.h:1129
Cluster::label
int label
Definition: omt_solver.h:20
Cluster::centroid
Eigen::Vector2d centroid
Definition: omt_solver.h:21
max
int64_t max(int64_t a, const int b)
Definition: Xin_Wang.cpp:10
compactParam
double compactParam
Definition: global.cpp:5
CDT
CGAL::Constrained_Delaunay_triangulation_2< K, TDS, Itag > CDT
Definition: global.h:48
Cluster::Cluster
Cluster()
Definition: omt_solver.h:23
DiscreteSolver::SolveOMT
bool SolveOMT(int mode=0)
Definition: omt_solver.h:1252
Cluster::Cluster
Cluster(const Cluster &other)
Definition: omt_solver.h:38
Cluster::Cluster
Cluster(int l, Eigen::Vector2d p, vector< int > s)
Definition: omt_solver.h:30
DiscreteSolver::DiscreteSolver
DiscreteSolver(DataEngine &de, CDT d, DistanceMetric &dm, vector< Eigen::Vector2d > s, vector< Eigen::Vector2d > t, double delta)
Definition: omt_solver.h:73
Eigen::internal::swap
void swap(scoped_array< T > &a, scoped_array< T > &b)
Definition: Memory.h:602
DiscreteSolver::m_delta
double m_delta
Definition: omt_solver.h:69
round
EIGEN_DEVICE_FUNC const RoundReturnType round() const
Definition: ArrayCwiseUnaryOps.h:374

co_scan
Author(s):
autogenerated on Mon Feb 28 2022 23:00:45