navigation.cpp
Go to the documentation of this file.
1 #include "navigation.h"
2 #include <cmath> // _isnan
3 #include <math.h> // _isnan
4 
5 using namespace std;
6 
7 // todo: seam
8 void Navigation::processCurrentScene()
9 {
10  // robot positions
11  {
12  m_robot_sites.clear();
13  for (int rid = 0; rid < m_p_de->m_pose2d.size(); ++rid)
14  m_robot_sites.push_back(Point_2(m_p_de->m_pose2d[rid].translation().x(), m_p_de->m_pose2d[rid].translation().y()));
15  }
16 
17  // current scene boundary...
18  {
19  // boundary
20  cv::Mat map_mat = m_p_de->visCellMap();
21  // find boundary
22  cv::Mat binary_mat(map_rows, map_cols, CV_8UC1);
23  for (int i = 0; i < binary_mat.rows; i++)
24  {
25  for (int j = 0; j < binary_mat.cols; j++)
26  {
27  binary_mat.ptr<uchar>(i)[j] = 0;
28  if (map_mat.ptr<cv::Vec3b>(i)[j][0] + map_mat.ptr<cv::Vec3b>(i)[j][1] + map_mat.ptr<cv::Vec3b>(i)[j][2] != 0)
29  binary_mat.ptr<uchar>(i)[j] = 255;
30  }
31  }
32  // dilate, to avoid self interact.
33  cv::Mat bd_gray_mat(map_rows, map_cols, CV_8UC1);
34  for (int i = 0; i < bd_gray_mat.rows; i++)
35  {
36  for (int j = 0; j < bd_gray_mat.cols; j++)
37  {
38  bd_gray_mat.ptr<uchar>(i)[j] = binary_mat.ptr<uchar>(i)[j];
39  }
40  }
41  cv::dilate(bd_gray_mat, bd_gray_mat, cv::getStructuringElement(0, cv::Size(3, 3))); // dilate.
42  cv::Mat bd_bin_mat;
43  cv::threshold(bd_gray_mat, bd_bin_mat, 0, 255, cv::THRESH_BINARY);
44  cv::dilate(bd_bin_mat, bd_bin_mat, cv::getStructuringElement(0, cv::Size(3, 3))); // dilate.
45  cv::erode(bd_bin_mat, bd_bin_mat, cv::getStructuringElement(0, cv::Size(3, 3))); // erode.
46  cv::erode(bd_bin_mat, bd_bin_mat, cv::getStructuringElement(0, cv::Size(3, 3))); // erode.
47  // contours
48  vector<vector<cv::Point>> bd_contours;
49  vector<vector<cv::Point>> dilate_contours_0;
50  cv::findContours(binary_mat, bd_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
51  cv::findContours(bd_bin_mat, dilate_contours_0, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
52  // find biggest contour
53  int dc_max_size_0 = 0;
54  int dc_max_index_0 = 0;
55  for (int i = 0; i < dilate_contours_0.size(); i++)
56  {
57  if (dilate_contours_0[i].size()>dc_max_size_0)
58  {
59  dc_max_size_0 = dilate_contours_0[i].size();
60  dc_max_index_0 = i;
61  }
62  }
63  // save boundary
64  m_boundary.clear();
65  m_boundary = dilate_contours_0[dc_max_index_0];
66 /*
67  // draw contours
68  cv::Mat pc_resultImage = cv::Mat::zeros(map_rows, map_cols, CV_8U);
69  cv::drawContours(pc_resultImage, dilate_contours_0, -1, cv::Scalar(255, 0, 255));
70  cv::imshow("contours", pc_resultImage);
71  cv::waitKey(0);
72 //*/
73  // find biggest contour (origin boundary to detect frontiers)
74  int c_max_size = 0;
75  int c_max_index = 0;
76  {
77  for (int i = 0; i < bd_contours.size(); i++)
78  {
79  if (bd_contours[i].size()>c_max_size){
80  c_max_size = bd_contours[i].size();
81  c_max_index = i;
82  }
83  }
84  }
85 
86  // frontiers
87  {
88  vector<cv::Point> samples_exploration; // samples of frontiers_exploration
89  vector<cv::Point> all_exploration; // all of frontiers_exploration
90  {
91  // frontier mat
92  cv::Mat frontier_mat = cv::Mat::zeros(map_rows, map_cols, CV_8UC1); // mat use to compute frontiers for exploration
93  // all frontiers for exploration
94  for (int contour_id = 0; contour_id < bd_contours.size(); contour_id++)
95  {
96  if (bd_contours[contour_id].size() <= 4)
97  continue;
98  for (int i = 0; i < bd_contours[contour_id].size(); i++)
99  {
100  if (m_p_de->m_recon2D.m_cellmap[bd_contours[contour_id][i].y][bd_contours[contour_id][i].x].isScanned && m_p_de->m_recon2D.m_cellmap[bd_contours[contour_id][i].y][bd_contours[contour_id][i].x].isFree)
101  { // 修改后: 筛选一些
102  bool is_frontier = true;
103  for (int r = -1; r <= 1; r++)
104  {
105  for (int c = -1; c <= 1; c++)
106  {
107  if (m_p_de->m_recon2D.m_cellmap[bd_contours[contour_id][i].y + r][bd_contours[contour_id][i].x + c].isScanned && m_p_de->m_recon2D.m_cellmap[bd_contours[contour_id][i].y + r][bd_contours[contour_id][i].x + c].isOccupied)
108  {
109  is_frontier = false;
110  break;
111  }
112  }
113  }
114  if (is_frontier)
115  frontier_mat.ptr<uchar>(bd_contours[contour_id][i].y)[bd_contours[contour_id][i].x] = 255;
116  }
117  }
118  }
119  // distance sample
120  for (int r = 0; r < map_rows; r++)
121  for (int c = 0; c < map_cols; c++)
122  if (frontier_mat.ptr<uchar>(r)[c] == 255)
123  all_exploration.push_back(cv::Point(c, r));
124  for (int i = 0; i < all_exploration.size(); i++) // sampling
125  {
126  // distance
127  double min_dis = DBL_MAX;
128  for (int tid = 0; tid < samples_exploration.size(); tid++) // compute distance from the closest frontier sample
129  {
130  Eigen::Vector2f pf(all_exploration[i].x, all_exploration[i].y);
131  Eigen::Vector2f pt(samples_exploration[tid].x, samples_exploration[tid].y);
132  double crt_dis = (pf - pt).norm();
133  if (__isnan(crt_dis) != 0)
134  crt_dis = 0;
135  if (crt_dis < min_dis)
136  min_dis = crt_dis;
137  }
138  // filter based on distance
139  if (min_dis > frontier_exploration_sample_range_pixel) //10 pixel = 0.5m, 20 initial
140  samples_exploration.push_back(cv::Point(all_exploration[i].x, all_exploration[i].y));
141  }
142  }
143  // save of all exploration
144  int ft_num = all_exploration.size();
145  m_frontiers.clear();
146  m_frontiers = all_exploration;
147  if (ft_num == 0)
148  {
149  cerr << "no frontier." << endl;
150  //getchar();
151  }
152  } // frontiers
153 
154  // holes
155  {
156  { // find holes
157  cv::Mat img = map_mat.clone();
158  for (int i = 0; i < img.rows; i++)
159  {
160  for (int j = 0; j < img.cols; j++)
161  {
162  img.ptr<cv::Vec3b>(i)[j][0] = 0;
163  img.ptr<cv::Vec3b>(i)[j][1] = 0;
164  }
165  }
166  cv::Mat gray_map(img.rows, img.cols, CV_8UC1);
167  cv::Mat bin_map;
168  cv::cvtColor(img, gray_map, CV_BGR2GRAY);
169  cv::threshold(gray_map, bin_map, 0, 255, cv::THRESH_BINARY);
170  // offset
171  dilate(bin_map, bin_map, cv::getStructuringElement(0, cv::Size(offset_size * 2 + 1, offset_size * 2 + 1))); // offset of obsticles, used to aviod collision between robot and object
172 
173 
174 
175 // todo: cut off scene outer boundary into pices
176  if (!g_scene_boundary.empty())
177  {
178  // select a split point
179  cv::Point mid( (g_scene_boundary[0].x + g_scene_boundary[1].x)/2, (g_scene_boundary[0].y + g_scene_boundary[1].y)/2 );
180  if (m_p_de->m_recon2D.m_cellmap[mid.y][mid.x].isScanned)
181  {
182  if (m_p_de->m_recon2D.m_cellmap[mid.y][mid.x].isOccupied)
183  {
184  int dx = abs(g_scene_boundary[0].x - g_scene_boundary[1].x);
185  int dy = abs(g_scene_boundary[0].y - g_scene_boundary[1].y);
186  if (dx>dy)
187  {
188  int min_x = mid.x-1,
189  max_x = mid.x+1,
190  min_y = mid.y-10,
191  max_y = mid.y+10;
192  for (int r = min_y; r <= max_y; r++)
193  for (int c = min_x; c <= max_x; c++)
194  bin_map.ptr<uchar>(r)[c] = 0;
195  char pth[100];
196  sprintf(pth, "result/obstical_%d.png", g_plan_iteration);
197  cv::imwrite(pth, bin_map);
198  }
199  else
200  {
201  int min_x = mid.x-10,
202  max_x = mid.x+10,
203  min_y = mid.y-1,
204  max_y = mid.y+1;
205  for (int r = min_y; r <= max_y; r++)
206  for (int c = min_x; c <= max_x; c++)
207  bin_map.ptr<uchar>(r)[c] = 0;
208  char pth[100];
209  sprintf(pth, "result/obstical_%d.png", g_plan_iteration);
210  cv::imwrite(pth, bin_map);
211  }
212  }
213  else
214  {
215  cerr<<"not a occupied voxel. continue?"<<endl;
216  //getchar(); getchar(); getchar();
217  }
218  }
219 /*
220  int min_x = col, min_y = row, max_x = 0, max_y = 0;
221  for (int i = 0; i < scene_seam.size(); i++)
222  {
223  if (scene_seam[i].x < min_x)
224  min_x = scene_seam[i].x;
225  if (scene_seam[i].x > max_x)
226  max_x = scene_seam[i].x;
227  if (scene_seam[i].y < min_y)
228  min_y = scene_seam[i].y;
229  if (scene_seam[i].y > max_y)
230  max_y = scene_seam[i].y;
231  }
232  for (int r = min_y; r <= max_y; r++)
233  for (int c = min_x; c <= max_x; c++)
234  bin_map.ptr<uchar>(r)[c] = 0;
235  char pth[100];
236  sprintf(pth, "data/offline/obstical%d.png", exe_count);
237  cv::imwrite(pth, bin_map);
238 //*/
239  }
240 /*
241  if (!scene_seam.empty())
242  {
243  int min_x = col, min_y = row, max_x = 0, max_y = 0;
244  for (int i = 0; i < scene_seam.size(); i++)
245  {
246  if (scene_seam[i].x < min_x)
247  min_x = scene_seam[i].x;
248  if (scene_seam[i].x > max_x)
249  max_x = scene_seam[i].x;
250  if (scene_seam[i].y < min_y)
251  min_y = scene_seam[i].y;
252  if (scene_seam[i].y > max_y)
253  max_y = scene_seam[i].y;
254  }
255  for (int r = min_y; r <= max_y; r++)
256  for (int c = min_x; c <= max_x; c++)
257  bin_map.ptr<uchar>(r)[c] = 0;
258  char pth[100];
259  sprintf(pth, "data/offline/obstical%d.png", exe_count);
260  cv::imwrite(pth, bin_map);
261  }
262 //*/
263  // contours
264  vector<vector<cv::Point>> contours;
265  vector<cv::Vec4i> hierarchy;
266  cv::findContours(bin_map, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_NONE); // tree
267  // filter hole in hole
268  vector<int> dlt_idx;
269  for (int i = 0; i < hierarchy.size(); i++)
270  {
271  if (hierarchy[i][3] != -1)
272  dlt_idx.push_back(i);
273  }
274  // delete hole in hole
275  if (dlt_idx.size() > 0){
276  for (int i = dlt_idx.size() - 1; i >= 0; i--)
277  {
278  //cout << "i: " << i << endl;
279  auto it = contours.begin() + dlt_idx[i];
280  contours.erase(it);
281  }
282  }
283  /*
284  // 判断是否只有一个大边界 hole
285  if (contours.size() == 1)
286  { // 判断是否是墙组成的大边界
287  int min_x = col, min_y = row;
288  int max_x = 0, max_y = 0;
289  for (int i = 0; i < contours[0].size(); i++)
290  {
291  if (contours[0][i].x < min_x)
292  min_x = contours[0][i].x;
293  if (contours[0][i].x > max_x)
294  max_x = contours[0][i].x;
295  if (contours[0][i].y < min_y)
296  min_y = contours[0][i].y;
297  if (contours[0][i].y > max_y)
298  max_y = contours[0][i].y;
299  }
300  if (max_y - min_y >= 190 && max_x - min_x >= 90)
301  { // todo: 如果是,把四周的墙都去掉一块把墙分开
302 
303  }
304  }
305  //*/
306  // check hole[i] size, remove which size<3. 2018-10-16
307  {
308  vector<int> deleteIndexes;
309  for (int hid = 0; hid < contours.size(); hid++)
310  if (contours[hid].size() < 3) // if contour size < 3, it cant be operated as polygon. on the other hand this contour is trival.
311  deleteIndexes.push_back(hid);
312  if (!deleteIndexes.empty())
313  for (int i = deleteIndexes.size() - 1; i >= 0; i--)
314  contours.erase(contours.begin() + deleteIndexes[i]);
315  }
316  // save
317  m_holes.clear();
318  m_holes = contours;
319  }
320  }
321  }
322 
323  return;
324 }
325 
326 // polygon simplification. reduce number of vertexes.
327 bool Navigation::simplifyPolygon(Polygon_2 & poly, const double colinearThresh, bool addNoise)
328 {
329  // check input
330  if (poly.size() < 3)
331  return false;
332  cerr << "polygon point number = " << poly.size() << endl;
333  // set up
334  vector<Eigen::Vector2d> polygon;
335  for (int i = 0; i < poly.size(); i++)
336  {
337  if (i >= 1)
338  {
339  Eigen::Vector2d lst(polygon.back());
340  Eigen::Vector2d crt(poly[i].x(), poly[i].y());
341  if ((crt - lst).norm() < 0.01)
342  continue;
343  }
344  polygon.push_back(Eigen::Vector2d(poly[i].x(), poly[i].y()));
345  }
346  // find keypoints
347  vector<int> keyIndexes;
348  for (int i = 0; i < polygon.size(); i++)
349  {
350  int last = (i - 1 + polygon.size()) % polygon.size();
351  int next = (i + 1) % polygon.size();
352  Eigen::Vector2d vec1 = polygon[i] - polygon[last];
353  vec1.normalize();
354  Eigen::Vector2d vec2 = polygon[next] - polygon[i];
355  vec2.normalize();
356  double dangle = acos(vec1.dot(vec2));
357  //if (1.000 - vec1.dot(vec2) < colinearThresh)
358  if (dangle < colinearThresh)
359  continue;
360  keyIndexes.push_back(i); // push a keypoint
361  }
362  // save keypoints polygon
363  vector<Point_2> newPolygon;
364  for (int i = 0; i < keyIndexes.size(); i++)
365  if (addNoise)
366  newPolygon.push_back(Point_2(polygon[keyIndexes[i]].x() + (double)rand() / RAND_MAX / 10, polygon[keyIndexes[i]].y() + (double)rand() / RAND_MAX / 10));
367  else
368  newPolygon.push_back(Point_2(polygon[keyIndexes[i]].x(), polygon[keyIndexes[i]].y()));
369  if (newPolygon.size() < 3)
370  {
371  cerr << "simplify error, polygon.size() = " << newPolygon.size() << endl;
372  return false;
373  }
374  poly = Polygon_2(newPolygon.begin(), newPolygon.end());
375  cerr << "simplified point number = " << poly.size() << endl;
376  //polygon_count++;
377  return true;
378 }
379 
380 // diff between polygons.
381 Pwh_list_2 Navigation::differenceCGALExactKernel(Polygon_with_holes_2 domain, Polygon_2 hole)
382 {
383  // set up
384  Pwh_list_2 res;
385  ofstream ofs;
386  ifstream ifs;
387  // save
388  ofs.open(domainFilePath);
389  ofs << domain;
390  ofs.clear();
391  ofs.close();
392  ofs.open(holeFilePath);
393  ofs << hole;
394  ofs.clear();
395  ofs.close();
396  ofs.open(communicateFile);
397  ofs << 0 << endl; // initialization
398  ofs.clear();
399  ofs.close();
400  // difference
401  system(poly_diff.data());
402  // result
403  int rtn;
404  ifs.open(communicateFile);
405  ifs >> rtn;
406  ifs.clear();
407  ifs.close();
408  int steps = 0;
409  while (rtn != 1)
410  {
411  if (rtn == 2)
412  {
413  cerr << "dsy: polygons difference result = 0" << endl;
414  Pwh_list_2 res;
415  res.push_back(domain);
416  return res;
417  }
418  //if (steps > 10) // test.
419  //{
420  // Pwh_list_2 res;
421  // res.push_back(domain);
422  // return res;
423  //}
424  ofs.open(communicateFile);
425  ofs << -1 << endl;
426  ofs.clear();
427  ofs.close();
428  system(poly_diff.data());
429  ifs.open(communicateFile);
430  ifs >> rtn;
431  ifs.clear();
432  ifs.close();
433  steps++;
434  }
435  // load
436  Polygon_with_holes_2 pass;
437  ifs.open(resultFilePath);
438  ifs >> pass;
439  ifs.clear();
440  ifs.close();
441  // format
442  res.push_back(pass);
443  return res;
444 }
445 
446 // load and check boundary.
447 Polygon_2 Navigation::load_and_check_boundary()
448 {
449  // set up
450  vector<Point_2> vertexes;
451  for (int i = 0; i < m_boundary.size(); i++)
452  vertexes.push_back(Point_2(m_boundary[i].x, -m_boundary[i].y));
453  Polygon_2 boundary(vertexes.begin(), vertexes.end());
454  if (boundary.area() < 0) // anti clock wise
455  boundary.reverse_orientation();
456  // check same points caused self intersection
457  bool self_intersect = false;
458  vector<Point_2> points;
459  set<Point_2> test_points;
460  for (size_t pid = 0; pid < boundary.size(); pid++)
461  {
462  if (test_points.find(Point_2(boundary[pid].x(), boundary[pid].y())) == test_points.end()){
463  test_points.insert(Point_2(boundary[pid].x(), boundary[pid].y()));
464  points.push_back(Point_2(boundary[pid].x(), boundary[pid].y()));
465  }
466  else{
467  self_intersect = true;
468  // 朝外侧扰动
469  double vx = boundary[pid].x() - boundary[pid - 1].x();
470  double vy = boundary[pid].y() - boundary[pid - 1].y();
471  double theta = -0.1; //逆时针排序时为负
472  vx = cos(theta)*vx - sin(theta)*vy;
473  vy = sin(theta)*vx + cos(theta)*vy;
474  double x = boundary[pid - 1].x() + vx;
475  double y = boundary[pid - 1].y() + vy;
476  test_points.insert(Point_2(x, y));
477  points.push_back(Point_2(x, y));
478  }
479  }
480  if (self_intersect)
481  boundary = Polygon_2(points.begin(), points.end());
482  if (boundary.area() < 0)
483  boundary.reverse_orientation();
484  // simplify boundary polygon
485  simplifyPolygon(boundary, 0.2, false);
486  // return.
487  return boundary;
488 }
489 
490 // load and check holes.
491 vector<Polygon_2> Navigation::load_and_check_holes(Polygon_2 boundary, vector<Polygon_2> & origin_holes)
492 {
493  // set up
494  vector<Polygon_2> holes;
495  origin_holes.clear();
496  vector<cv::Point> bdry;
497  for (int i = 0; i < boundary.size(); i++)
498  bdry.push_back(cv::Point(round(boundary[i].x()), -round(boundary[i].y())));
499  // load holes
500  for (int hid = 0; hid < m_holes.size(); hid++)
501  {
502  Polygon_2 p_pass;
503  for (int vid = 0; vid < m_holes[hid].size(); vid++)
504  {
505  p_pass.push_back(Point_2(m_holes[hid][vid].x, -m_holes[hid][vid].y));
506  }
507  origin_holes.push_back(p_pass);
508  }
509  // compute holes
510  {
511  // holes赋值
512  for (int i = 0; i < origin_holes.size(); i++)
513  {
514  if (origin_holes[i].area() > 0)
515  origin_holes[i].reverse_orientation(); // 需要顺时针排序即面积为负
516  holes.push_back(origin_holes[i]);
517  }
518  // 对于每个hole判断其边界是否自交
519  for (int hid = 0; hid < holes.size(); hid++)
520  {
521  bool self_intersect = false;
522  vector<Point_2> points;
523  set<Point_2> test_points;
524  for (int pid = 0; pid < holes[hid].size(); pid++)
525  {
526  if (test_points.find(Point_2(holes[hid][pid].x(), holes[hid][pid].y())) == test_points.end())
527  {
528  test_points.insert(Point_2(holes[hid][pid].x(), holes[hid][pid].y()));
529  points.push_back(Point_2(holes[hid][pid].x(), holes[hid][pid].y()));
530  }
531  else
532  {
533  self_intersect = true;
534  // 朝外侧扰动
535  double vx = holes[hid][pid].x() - holes[hid][pid - 1].x();
536  double vy = holes[hid][pid].y() - holes[hid][pid - 1].y();
537  double theta = 0.1;//顺时针排序时为正
538  vx = cos(theta)*vx - sin(theta)*vy;
539  vy = sin(theta)*vx + cos(theta)*vy;
540  double x = holes[hid][pid - 1].x() + vx;
541  double y = holes[hid][pid - 1].y() + vy;
542  test_points.insert(Point_2(x, y));
543  points.push_back(Point_2(x, y));
544  }
545  }
546  if (self_intersect)
547  holes[hid] = Polygon_2(points.begin(), points.end());
548  if (holes[hid].area() < 0)
549  holes[hid].reverse_orientation();
550  }
551  }
552  // simplify
553  for (int hid = 0; hid < holes.size(); hid++)
554  {
555  simplifyPolygon(holes[hid], 0.1, true);
556  }
557  // end.
558  return holes;
559 }
560 
561 // done.
562 void Navigation::correct_boundary_holes(Polygon_2 & boundary, std::vector<Polygon_2> & holes)
563 {
564  // pre check
565  vector<int> dlt_idx;
566  for (size_t hid = 0; hid < holes.size(); hid++)
567  if (holes[hid].size() == 0)
568  dlt_idx.push_back(hid);
569  if (!dlt_idx.empty())
570  {
571  for (int id = dlt_idx.size() - 1; id >= 0; id--)
572  {
573  holes.erase(holes.begin() + dlt_idx[id]);
574  }
575  }
576  if (holes.empty())
577  {
578  cerr << "error, holes empty" << endl;
579  }
580  // domain
581  Pwh_list_2 results;
582  Polygon_with_holes_2 domain;
583  domain.outer_boundary() = boundary;
584  for (int hid = 0; hid < holes.size(); hid++)
585  {
586  // difference use exact kernel to avoid numerical error
587  results = differenceCGALExactKernel(domain, holes[hid]);
588  //cerr << "results(polygons) size = " << results.size() << endl;
589  //cerr << "outer boundary area = " << results.begin()->outer_boundary().area() << endl;
590  // update domain
591  if (results.size() != 1
592  ){ // if domain become not continious
593  int max_size = 0;
594  for (auto i = results.begin(); i != results.end(); i++)
595  if (i->outer_boundary().size() > max_size)
596  max_size = i->outer_boundary().size();
597  for (auto i = results.begin(); i != results.end(); i++)
598  {
599  if (i->outer_boundary().size() == max_size)
600  {
601  domain = Polygon_with_holes_2(*i);
602  break;
603  }
604  }
605  results.clear();
606  continue;
607  }
608  domain = Polygon_with_holes_2(*results.begin());
609  results.clear();
610  }
611  // reset boundary and holes
612  boundary.clear();
613  boundary = Polygon_2(domain.outer_boundary());
614  holes.clear();
615  for (auto i = domain.holes_begin(); i != domain.holes_end(); i++)
616  holes.push_back(*i);
617  // 防止数值错误的扰动
618  {
619  vector<Point_2> pass_polygon;
620  for (int pid = 0; pid < boundary.size(); pid++)
621  pass_polygon.push_back(Point_2(boundary[pid].x() + (rand() / RAND_MAX) / 10, boundary[pid].y() + (rand() / RAND_MAX) / 10));
622  boundary = Polygon_2(pass_polygon.begin(), pass_polygon.end());
623  // 判断boundary的边界是否自交
624  bool self_intersect = false;
625  vector<Point_2> points;
626  set<Point_2> test_points;
627  for (size_t pid = 0; pid < boundary.size(); pid++)
628  {
629  if (test_points.find(Point_2(boundary[pid].x(), boundary[pid].y())) == test_points.end())
630  {
631  test_points.insert(Point_2(boundary[pid].x(), boundary[pid].y()));
632  points.push_back(Point_2(boundary[pid].x(), boundary[pid].y()));
633  }
634  else
635  {
636  self_intersect = true;
637  // 朝外侧扰动
638  double vx = boundary[pid].x() - boundary[pid - 1].x();
639  double vy = boundary[pid].y() - boundary[pid - 1].y();
640  double theta = -0.1;//逆时针排序时为负
641  vx = cos(theta)*vx - sin(theta)*vy;
642  vy = sin(theta)*vx + cos(theta)*vy;
643  double x = boundary[pid - 1].x() + vx;
644  double y = boundary[pid - 1].y() + vy;
645  test_points.insert(Point_2(x, y));
646  points.push_back(Point_2(x, y));
647  }
648  }
649  if (self_intersect)
650  boundary = Polygon_2(points.begin(), points.end());
651  if (boundary.area() < 0)
652  boundary.reverse_orientation();
653  }
654  // end.
655  return;
656 }
657 
658 // generate robot move domain.
659 bool Navigation::robotMoveDomainProcess(Polygon_2 & boundary, vector<Polygon_2> & holes, vector<Polygon_2> & origin_holes)
660 {
661  // domain boundary
662  boundary = load_and_check_boundary();
663  // domain holes
664  holes = load_and_check_holes(boundary, origin_holes);
665  // correction: compute domain and reset boundary & holes
666  correct_boundary_holes(boundary, holes); // by the difference of polygons
667  // end.
668  return true;
669 }
670 
671 // cgal: mark_domains
672 void
673 mark_domains(CDT& ct,
674 CDT::Face_handle start,
675 int index,
676 std::list<CDT::Edge>& border)
677 {
678  if (start->info().nesting_level != -1){
679  return;
680  }
681  std::list<CDT::Face_handle> queue;
682  queue.push_back(start);
683  while (!queue.empty()){
684  CDT::Face_handle fh = queue.front();
685  queue.pop_front();
686  if (fh->info().nesting_level == -1){
687  fh->info().nesting_level = index;
688  for (int i = 0; i < 3; i++){
689  CDT::Edge e(fh, i);
690  CDT::Face_handle n = fh->neighbor(i);
691  if (n->info().nesting_level == -1){
692  if (ct.is_constrained(e)) border.push_back(e);
693  else queue.push_back(n);
694  }
695  }
696  }
697  }
698 }
699 
700 // cgal: mark_domains
701 void
702 mark_domains(CDT& cdt)
703 {
704  for (CDT::All_faces_iterator it = cdt.all_faces_begin(); it != cdt.all_faces_end(); ++it){
705  it->info().nesting_level = -1;
706  }
707  std::list<CDT::Edge> border;
708  mark_domains(cdt, cdt.infinite_face(), 0, border);
709  while (!border.empty()){
710  CDT::Edge e = border.front();
711  border.pop_front();
712  CDT::Face_handle n = e.first->neighbor(e.second);
713  if (n->info().nesting_level == -1){
714  mark_domains(cdt, n, e.first->info().nesting_level + 1, border);
715  }
716  }
717 }
718 
719 // save to obj and off
720 void saveCDT_geodesic(CDT cdt)
721 { // save mesh
722  vector<int> rbt_sites_indexes;
723  vector<Point_2> vertices; // vertices in domain
724  // filter vertices in domain
725  {
726  vector<Point_2> vertices_all;
727  for (CDT::Finite_vertices_iterator vit = cdt.finite_vertices_begin();
728  vit != cdt.finite_vertices_end(); ++vit){
729  vertices_all.push_back(Point_2(vit->point().x(), vit->point().y()));
730  }
731  //cout << vertices_all.size() << endl;
732  // flag: to remain only indomain vertices
733  //int flag[10000];
734  int* flag = new int[60000];
735  for (size_t i = 0; i < vertices_all.size(); i++)
736  {
737  flag[i] = 0;
738  }
739  for (CDT::Finite_faces_iterator fit = cdt.finite_faces_begin();
740  fit != cdt.finite_faces_end(); ++fit)
741  {
742  if (fit->info().in_domain()) {
743  for (size_t i = 0; i < 3; i++)
744  {
745  for (size_t j = 0; j < vertices_all.size(); j++)
746  {
747  if (fit->vertex(i)->point() == vertices_all[j]){
748  flag[j] = 1;
749  break;
750  }
751  }
752  }
753  }
754  }
755  for (size_t i = 0; i < vertices_all.size(); i++)
756  {
757  if (flag[i] == 1){
758  vertices.push_back(Point_2(vertices_all[i].x(), vertices_all[i].y()));
759  }
760  }
761  delete[] flag;
762  }
763  // save cdt to obj file: use for interior distance
764  {
765  ofstream obj_out(cdt_obj_path);
766  //cerr << "saving obj file..."; // 12-25
767  {// write to obj file
768  for (size_t i = 0; i < vertices.size(); i++)
769  {
770  obj_out << "v " << vertices[i].x() << " " << vertices[i].y() << " 0" << endl;
771  }
772  // write faces
773  for (CDT::Finite_faces_iterator fit = cdt.finite_faces_begin();
774  fit != cdt.finite_faces_end(); ++fit)
775  {
776  if (fit->info().in_domain()) {
777  obj_out << "f ";
778  for (size_t i = 0; i < 3; i++)
779  {
780  for (size_t j = 0; j < vertices.size(); j++)
781  {
782  if (fit->vertex(i)->point() == vertices[j]){
783  obj_out << j + 1 << " ";
784  break;
785  }
786  }
787  }
788  obj_out << endl;
789  }
790  }
791  }
792  obj_out.close();
793  //cerr << "done." << endl; // 12-25
794  //getchar(); // 12-25
795  }
796 /*
797  // save cdt to off file: use for polyhedron
798  {
799  ofstream off_out;
800  off_out.open(cdt_off_path);
801  off_out << "OFF" << endl;
802  // vertices
803  vector<Point_2> vertices_all;
804  for (CDT::Finite_vertices_iterator vit = cdt.finite_vertices_begin();
805  vit != cdt.finite_vertices_end(); ++vit){
806  vertices_all.push_back(Point_2(vit->point().x(), vit->point().y()));
807  }
808  //cout << vertices_all.size() << endl;
809  // flag: to remain only indomain vertices
810  //int flag[10000];
811  int* flag = new int[60000];
812  for (size_t i = 0; i < vertices_all.size(); i++)
813  {
814  flag[i] = 0;
815  }
816  for (CDT::Finite_faces_iterator fit = cdt.finite_faces_begin();
817  fit != cdt.finite_faces_end(); ++fit)
818  {
819  if (fit->info().in_domain()) {
820  for (size_t i = 0; i < 3; i++)
821  {
822  for (size_t j = 0; j < vertices_all.size(); j++)
823  {
824  if (fit->vertex(i)->point() == vertices_all[j]){
825  flag[j] = 1;
826  break;
827  }
828  }
829  }
830  }
831  }
832  vector<Point_2> vertices;
833  for (size_t i = 0; i < vertices_all.size(); i++)
834  {
835  if (flag[i] == 1){
836  vertices.push_back(Point_2(vertices_all[i].x(), vertices_all[i].y()));
837  }
838  }
839  delete[] flag;
840  //cout << vertices.size() << endl;
841  int f_count = 0;
842  for (CDT::Finite_faces_iterator fit = cdt.finite_faces_begin();
843  fit != cdt.finite_faces_end(); ++fit)
844  if (fit->info().in_domain())
845  ++f_count;
846  off_out << vertices.size() << " " << f_count << " 0" << endl;
847  // write vertices
848  for (size_t i = 0; i < vertices.size(); i++)
849  {
850  off_out << vertices[i].x() << " " << vertices[i].y() << " 0" << endl;
851  }
852  // write f
853  for (CDT::Finite_faces_iterator fit = cdt.finite_faces_begin();
854  fit != cdt.finite_faces_end(); ++fit)
855  {
856  if (fit->info().in_domain()) {
857  off_out << "3 ";
858  for (size_t i = 0; i < 3; i++)
859  {
860  for (size_t j = 0; j < vertices.size(); j++)
861  {
862  if (fit->vertex(i)->point() == vertices[j]){
863  off_out << j << " ";
864  break;
865  }
866  }
867  }
868  off_out << endl;
869  }
870  }
871  off_out.close();
872  }
873 //*/
874  return;
875 }
876 
877 // CDT.
878 bool Navigation::generateMoveDomainCDT(Polygon_2 boundary, vector<Polygon_2> holes, vector<Polygon_2> origin_holes, vector<ScanningTask> tasks, CDT & cdt)
879 {
880  set<Point_2> tmpSet; // sites in cdt: include boundary, holes, tasks, rbts and random
881  { // Insert the polygons into a constrained triangulation
882  // set boundary
883  cdt.insert_constraint(boundary.vertices_begin(), boundary.vertices_end(), true);
884  // insert boundary points
885  for (size_t i = 0; i < boundary.size(); i++)
886  {
887  tmpSet.insert(Point_2(boundary[i].x(), boundary[i].y()));
888  }
889  // set holes
890  for (size_t id = 0; id < holes.size(); id++)
891  cdt.insert_constraint(holes[id].vertices_begin(), holes[id].vertices_end(), true);
892  // insert holes points
893  for (size_t i = 0; i < holes.size(); i++)
894  {
895  for (size_t j = 0; j < holes[i].size(); j++)
896  {
897  tmpSet.insert(Point_2(holes[i][j].x(), holes[i][j].y()));
898  }
899  }
901  //for (size_t i = 0; i < frontiers.size(); i++)
902  //{
903  // tmpSet.insert(Point_2(frontiers[i].x(), frontiers[i].y()));
904  //}
905  // insert task points
906  for (size_t i = 0; i < tasks.size(); i++)
907  {
908  tmpSet.insert(Point_2(tasks[i].view.pose.translation().x(), tasks[i].view.pose.translation().y()));
909  }
910  // set robot sites
911  {
912  // check if in hole
913  {
914  for (size_t i = 0; i < m_robot_sites.size(); i++)
915  {
916  for (size_t j = 0; j < holes.size(); j++)
917  {
918  CGAL::Bounded_side bs = CGAL::bounded_side_2(holes[j].vertices_begin(), holes[j].vertices_end(), m_robot_sites[i]);
919  if (bs == CGAL::ON_BOUNDED_SIDE)
920  {
921  // 如果在障碍物（原始）内部，报错
922  if (CGAL::bounded_side_2(origin_holes[j].vertices_begin(), origin_holes[j].vertices_end(), m_robot_sites[i]) == CGAL::ON_BOUNDED_SIDE)
923  {
924  cerr << "error, move into a hole" << endl;
925  }
926  // find closest point
927  double min_dis = DBL_MAX;
928  int min_index = -1;
929  for (size_t k = 0; k < holes[j].size(); k++)
930  {
931  if (
932  sqrt((holes[j][k].x() - m_robot_sites[i].x())*(holes[j][k].x() - m_robot_sites[i].x())
933  + (holes[j][k].y() - m_robot_sites[i].y())*(holes[j][k].y() - m_robot_sites[i].y()))
934  < min_dis
935  )
936  {
937  min_dis = sqrt((holes[j][k].x() - m_robot_sites[i].x())*(holes[j][k].x() - m_robot_sites[i].x())
938  + (holes[j][k].y() - m_robot_sites[i].y())*(holes[j][k].y() - m_robot_sites[i].y()));
939  min_index = k;
940  }
941  }
942  // 改为边缘处的点
943  m_robot_sites[i] = Point_2(holes[j][min_index].x(), holes[j][min_index].y());
944  }
945  }
946  }
947  }
948  }
949  // set tssk sites
950  {
951  for (int i = 0; i < tasks.size(); i++)
952  tmpSet.insert(Point_2(tasks[i].view.pose.translation().x(), tasks[i].view.pose.translation().y()));
953  }
954  // set m_robot_sites and random sites
955  {
956  for (size_t i = 0; i < m_robot_sites.size(); i++)
957  tmpSet.insert(Point_2(m_robot_sites[i].x(), m_robot_sites[i].y()));
958  {// ragular random sites
959  CGAL::Bbox_2 box = boundary.bbox();
960  int site_num = ceil(sqrt(random_site_num));
961  double delta_x = (box.xmax() - box.xmin()) / site_num;
962  double delta_y = (box.ymax() - box.ymin()) / site_num;
963  for (int i = 0; i < site_num; i++)
964  {
965  for (int j = 0; j < site_num; j++)
966  {
967  double x = box.xmin() + i*delta_x + (double)(rand() % 10) / 6;
968  double y = box.ymin() + j*delta_y + (double)(rand() % 10) / 6;
969  Point_2 pt(x, y);
970  //cout << "plot(" << x << ", " << y << ", 'bo'); hold on;" << endl;
971  bool rand_ok = true;
972  if (CGAL::bounded_side_2(boundary.vertices_begin(), boundary.vertices_end(), pt) != CGAL::ON_BOUNDED_SIDE)
973  rand_ok = false;
974  for (size_t hid = 0; hid < holes.size(); hid++)
975  if (CGAL::bounded_side_2(holes[hid].vertices_begin(), holes[hid].vertices_end(), pt) != CGAL::ON_UNBOUNDED_SIDE)
976  {
977  rand_ok = false;
978  break;
979  }
980  //if (CGAL::bounded_side_2(boundary.vertices_begin(), boundary.vertices_end(), pt) == CGAL::ON_BOUNDED_SIDE)
981  if (rand_ok)
982  {
983  double distanceSquare = FLT_MAX;
984  for (set<Point_2>::const_iterator it = tmpSet.begin(); it != tmpSet.end(); ++it)
985  {
986  if ((*it - pt).squared_length() < distanceSquare)
987  distanceSquare = (*it - pt).squared_length();
988  }
989  if (distanceSquare > 2e-3 * 2e-3)
990  tmpSet.insert(pt);
991  }
992  }
993  }
994  }
995  }
996  // insert sites
997  //cdt.insert(tmpSet.begin(), tmpSet.end(), false); // origin code.
998  cdt.insert(tmpSet.begin(), tmpSet.end()); // linux func.. is it ok?
999  // Mark facets that are inside the domain bounded by the polygon
1000  mark_domains(cdt);
1001  }
1002  return true;
1003 }
1004 
1005 // load and check frontiers, saved in the variable frontiers
1006 vector<Point_2> Navigation::load_and_check_frontiers(Polygon_2 & boundary, vector<Polygon_2> & holes)
1007 {
1008  for (int fid = 0; fid < m_frontiers.size(); ++fid)
1009  {
1010  m_frontiers_p2.push_back(Point_2(m_frontiers[fid].x, -m_frontiers[fid].y));
1011  }
1012  return m_frontiers_p2;
1013 }
1014 
1015 // remove invalid frontiers, not finish
1016 vector<int> Navigation::preprocess_frontiers(Polygon_2 outer, vector<Polygon_2> iner)
1017 {
1018  // set up
1019  m_frontierList.clear();
1020  vector<int> indexes;
1021  vector<Point_2> tasks;
1022  vector<cv::Point> boundary;
1023  for (int pid = 0; pid < outer.size(); pid++)
1024  {
1025  boundary.push_back(cv::Point(round(outer[pid].x()), -round(outer[pid].y())));
1026  }
1027  vector<vector<cv::Point>> holes;
1028  for (int hid = 0; hid < iner.size(); hid++)
1029  {
1030  vector<cv::Point> hole;
1031  for (int pid = 0; pid < iner[hid].size(); pid++)
1032  {
1033  hole.push_back(cv::Point(round(iner[hid][pid].x()), -round(iner[hid][pid].y())));
1034  }
1035  holes.push_back(hole);
1036  }
1037  // determine if the frontier is a task
1038  for (int i = 0; i < m_frontiers_p2.size(); i++)
1039  {
1040  // check reachable
1041  bool visible = true;
1042  string err = "";
1043  // voxels that cant scan
1044  if (visible)
1045  for (auto unvisible_task = task_maybe_invalid.begin(); unvisible_task != task_maybe_invalid.end(); unvisible_task++)
1046  {
1047  if (fabs(m_frontiers_p2[i].x() - unvisible_task->x()) < 1 && fabs(m_frontiers_p2[i].y() - unvisible_task->y()) < 1)
1048  {
1049  visible = false;
1050  err = "cant scan.";
1051  break;
1052  }
1053  }
1054  // voxels that out of boundary
1055  if (visible)
1056  if (cv::pointPolygonTest(boundary, cv::Point(round(m_frontiers_p2[i].x()), -round(m_frontiers_p2[i].y())), false) < 0)
1057  {
1058  // if distance < offset_size, its may be valid.
1059  double d = fabs(cv::pointPolygonTest(boundary, cv::Point(round(m_frontiers_p2[i].x()), -round(m_frontiers_p2[i].y())), true));
1060  if (d > offset_size*1.41)
1061  {
1062  visible = false;
1063  err = "out of boundary.";
1064  }
1065  }
1066  // voxels that in holes
1067  if (visible)
1068  for (int hid = 0; hid < holes.size(); hid++)
1069  {
1070  if (cv::pointPolygonTest(holes[hid], cv::Point(round(m_frontiers_p2[i].x()), -round(m_frontiers_p2[i].y())), false) > 0)
1071  {
1072  // if distance < offset_size, its may be valid.
1073  double d = fabs(cv::pointPolygonTest(holes[hid], cv::Point(round(m_frontiers_p2[i].x()), -round(m_frontiers_p2[i].y())), true));
1074  if (d > (offset_size-1)*1.41)
1075  {
1076  visible = false;
1077  err = "in hole.";
1078  break;
1079  }
1080  //visible = false;
1081  //err = "in hole.";
1082  //break;
1083  }
1084  }
1085  // voxels that out of scene boundary
1086  if (visible)
1087  if (!g_scene_boundary.empty()) // 20200311
1088  {
1089  //if (cv::pointPolygonTest(g_scene_boundary, cv::Point(round(m_frontiers_p2[i].x()), -round(m_frontiers_p2[i].y())), false) < 0)
1090  if (cv::pointPolygonTest(g_scene_boundary, cv::Point(round(m_frontiers_p2[i].x()), -round(m_frontiers_p2[i].y())), false) <= 0)
1091  {
1092  visible = false;
1093  err = "out of scene_boundary.";
1094  }
1095  else if(cv::pointPolygonTest(g_scene_boundary, cv::Point(round(m_frontiers_p2[i].x()), -round(m_frontiers_p2[i].y())), true) < 2) // < 3
1096  {
1097  visible = false;
1098  err = "out of scene_boundary. too close to boundary.";
1099  }
1100  }
1101 //*/
1102  // voxels that trivial
1103  if (visible)
1104  {
1105  const int delta = 3;
1106  const int thresh = 6; // default 7 is ok.
1107  int unknownCounter = 0;
1108  cv::Point vox(round(m_frontiers_p2[i].x()), -round(m_frontiers_p2[i].y()));
1109  int beg_r = vox.y - delta < 0 ? 0 : vox.y - delta;
1110  int end_r = vox.y + delta > map_rows - 1 ? map_rows - 1 : vox.y + delta;
1111  int beg_c = vox.x - delta < 0 ? 0 : vox.x - delta;
1112  int end_c = vox.x + delta > map_cols - 1 ? map_cols - 1 : vox.x + delta;
1113  for (int r = beg_r; r <= end_r; r++)
1114  for (int c = beg_c; c <= end_c; c++)
1115  if (!m_p_de->m_recon2D.m_cellmap[r][c].isScanned)
1116  unknownCounter++;
1117  if (unknownCounter <= thresh)
1118  {
1119  visible = false;
1120  err = "trivial.";
1121  }
1122  }
1123 /*
1124 // todo: confidence map 2d
1125  // if unvisible, continue
1126  if (!visible)
1127  {
1128  int r = -(int)round(m_frontiers_p2[i].y());
1129  int c = (int)round(m_frontiers_p2[i].x());
1130  confidence_map_2d[r][c] = confidence_max;
1131  continue;
1132  }
1133 //*/
1134 
1135  // store visible frontier to m_frontierList
1136 /* origin code
1137  tasks.push_back(Point_2(m_frontiers_p2[i].x(), m_frontiers_p2[i].y()));
1138  indexes.push_back(i);
1139  FrontierElement fe(Point_2(m_frontiers_p2[i].x(), m_frontiers_p2[i].y()));
1140  m_frontierList.push_back(fe);
1141 //*/
1142 
1143  // dsy 20-03-21
1144  if (visible)
1145  {
1146  cv::Point crt_p(round(m_frontiers_p2[i].x()), -round(m_frontiers_p2[i].y()));
1147  bool already_explored = true;
1148  int radius = 1;
1149  for (int dr = -radius; dr <= radius; ++dr)
1150  {
1151  for (int dc = -radius; dc <= radius; ++dc)
1152  {
1153  if (!m_p_de->m_recon2D.m_cellmap[crt_p.y+dr][crt_p.x+dc].isScanned)
1154  {
1155  already_explored = false;
1156  }
1157  }
1158  }
1159  if (already_explored)
1160  {
1161  visible = false;
1162  err = 'already explored.';
1163  }
1164  }
1165 
1166  // dsy 19-10-07
1167  if (visible)
1168  {
1169  tasks.push_back(Point_2(m_frontiers_p2[i].x(), m_frontiers_p2[i].y()));
1170  indexes.push_back(i);
1171  FrontierElement fe(Point_2(m_frontiers_p2[i].x(), m_frontiers_p2[i].y()));
1172  m_frontierList.push_back(fe);
1173  }
1174 
1175  }
1176 //*/
1177  return indexes;
1178 }
1179 
1180 // compute location map
1181 cv::Mat Navigation::computeLoationMap()
1182 {
1183  // set up
1184  const int thresh = 30;
1185  const int kernelR = 3;
1186  // input
1187  cv::Mat img = m_p_de->visCellMap();
1188  // free space
1189  cv::Mat freeSpaces = cv::Mat::zeros(img.rows, img.cols, CV_8UC1);
1190  for (int r = 0; r < img.rows; r++)
1191  {
1192  for (int c = 0; c < img.cols; c++)
1193  {
1194  if (img.ptr<cv::Vec3b>(r)[c][1] == 255)
1195  freeSpaces.ptr<uchar>(r)[c] = 10;
1196  }
1197  }
1198  // location map
1199  cv::Mat LocationMap = cv::Mat::zeros(img.rows, img.cols, CV_8UC1);
1200  LocationMap += freeSpaces;
1201  // computing
1202  for (int step = 0; step < thresh; step++)
1203  {
1204  cv::erode(freeSpaces, freeSpaces, cv::getStructuringElement(0, cv::Size(kernelR * 2, kernelR * 2)));
1205  LocationMap += freeSpaces;
1206  }
1207  return LocationMap;
1208 }
1209 
1210 // check view ray validness(without occlusion) 2018-09-12. not finish
1211 bool Navigation::checkViewRayValidness(cv::Point source, cv::Point target)
1212 {
1213  // set up
1214  const int invalidValue = 255;
1215  cv::Point beg(source);
1216  cv::Point end(target);
1217  cv::Mat domain = cv::Mat::zeros(map_rows, map_cols, CV_8UC1);
1218  for (int r = 0; r < domain.rows; r++)
1219  {
1220  for (int c = 0; c < domain.cols; c++)
1221  {
1222  // not allow view through unknown
1223  if (m_p_de->m_recon2D.m_cellmap[r][c].isScanned && m_p_de->m_recon2D.m_cellmap[r][c].isFree)
1224  {
1225  // free spaces value '0'
1226  }
1227  else
1228  {
1229  // unknown and occupied spaces value 'invalidValue'
1230  domain.ptr<uchar>(r)[c] = invalidValue;
1231  }
1232  }
1233  }
1234  // DDA
1235  bool occlusion = false;
1236  int dx = end.x - beg.x;
1237  int dy = end.y - beg.y;
1238  if (dx == 0)
1239  {
1240  int beg_y = beg.y < end.y ? beg.y : end.y;
1241  int end_y = beg.y > end.y ? beg.y : end.y;
1242  for (int y = beg_y; y < end_y; y++)
1243  if (domain.ptr<uchar>(y)[beg.x] == invalidValue)
1244  {
1245  occlusion = true;
1246  break;
1247  }
1248  }
1249  else if (dy == 0)
1250  {
1251  int beg_x = beg.x < end.x ? beg.x : end.x;
1252  int end_x = beg.x > end.x ? beg.x : end.x;
1253  for (int x = beg_x; x < end_x; x++)
1254  if (domain.ptr<uchar>(beg.y)[x] == invalidValue)
1255  {
1256  occlusion = true;
1257  break;
1258  }
1259  }
1260  else if (dx == 0 && dy == 0){}
1261  else
1262  {
1263  int MaxStep = abs(dx) > abs(dy) ? abs(dx) : abs(dy); // iteration step
1264  double fXUnitLen = 1.0; // delta x
1265  double fYUnitLen = 1.0; // delta y
1266  fYUnitLen = (double)(dy) / (double)(MaxStep);
1267  fXUnitLen = (double)(dx) / (double)(MaxStep);
1268  double x = (double)(beg.x);
1269  double y = (double)(beg.y);
1270  // step
1271  for (long i = 1; i < MaxStep; i++) // '<' or '<=', not include end point
1272  {
1273  x = x + fXUnitLen;
1274  y = y + fYUnitLen;
1275  if (domain.ptr<uchar>((int)round(y))[(int)round(x)] == invalidValue)
1276  {
1277  occlusion = true;
1278  break;
1279  }
1280  }
1281  }
1282  return !occlusion; // return valid if not occluded
1283 }
1284 
1285 // frustum contour.
1286 vector<cv::Point> Navigation::get_frustum_contuor(iro::SE2 pose)
1287 {
1288  // set up
1289  Eigen::Vector2d position(pose.translation().x(), pose.translation().y());
1290  Eigen::Vector2d direction(0, 1);
1291  {
1292  double angle = pose.rotation().angle();
1293  Eigen::Matrix2d r;
1294  r << cos(angle), -sin(angle),
1295  sin(angle), cos(angle);
1296  direction = r*direction;
1297  }
1298  //cerr << "position = " << position.transpose() << endl;
1299  //cerr << "direction = " << direction.transpose() << endl;
1300  // range
1301  double scale = (double)m_p_de->scan_max_range / 1000 / map_cellsize;
1302  scale /= cos(PI / 6);
1303  direction = scale*direction;
1304  // left end point
1305  Eigen::Vector2d lp;
1306  {
1307  Eigen::Vector2d vec(direction);
1308  double angle = PI / 6;
1309  Eigen::Matrix2d r;
1310  r << cos(angle), -sin(angle),
1311  sin(angle), cos(angle);
1312  vec = r*vec;
1313  lp = position + vec;
1314  }
1315  //cerr << "lp = " << lp.transpose() << endl;
1316  // right end point
1317  Eigen::Vector2d rp;
1318  {
1319  Eigen::Vector2d vec(direction);
1320  double angle = -PI / 6;
1321  Eigen::Matrix2d r;
1322  r << cos(angle), -sin(angle),
1323  sin(angle), cos(angle);
1324  vec = r*vec;
1325  rp = position + vec;
1326  }
1327  //cerr << "rp = " << rp.transpose() << endl;
1328  // coor sys trans
1329  cv::Point p_o((int)round(position.x()), -(int)round(position.y()));
1330  cv::Point p_l((int)round(lp.x()), -(int)round(lp.y()));
1331  cv::Point p_r((int)round(rp.x()), -(int)round(rp.y()));
1332  //cerr << "p_o = " << p_o << endl;
1333  //cerr << "p_l = " << p_l << endl;
1334  //cerr << "p_r = " << p_r << endl;
1335  vector<cv::Point> result;
1336  result.push_back(p_o);
1337  result.push_back(p_l);
1338  result.push_back(p_r);
1339  return result;
1340 }
1341 
1342 // NBV for frontiers
1343 vector<NextBestView> Navigation::generateViewsFrontiers(bool enableSampling, double sampleRate)
1344 {
1345  // if srand
1346  srand((unsigned)time(NULL));
1347  // set up
1348  vector<NextBestView> nbvs;
1349  vector<FrontierElement> frontier_list;
1350  for (int i = 0; i < m_frontierList.size(); i++)
1351  {
1352  if (enableSampling)
1353  {
1354  if ((double)rand() / RAND_MAX < sampleRate)
1355  {
1356  frontier_list.push_back(m_frontierList[i]);
1357  }
1358  }
1359  else
1360  {
1361  frontier_list.push_back(m_frontierList[i]);
1362  }
1363  }
1364 
1365  // test record sampled frontier
1366  {
1367  cv::Mat statement = m_p_de->visCellMap();
1368  cv::Mat temp = statement.clone();
1369  // draw frontiers in list
1370  for (int fid = 0; fid < m_frontierList.size(); fid++)
1371  {
1372  int x = m_frontierList[fid].position.x();
1373  int y = -m_frontierList[fid].position.y();
1374  cv::circle(temp, cv::Point(x, y), 0, CV_RGB(255, 255, 255));
1375  cv::circle(temp, cv::Point(x, y), 1, CV_RGB(255, 255, 255));
1376  cv::circle(temp, cv::Point(x, y), 2, CV_RGB(255, 255, 255));
1377  }
1378  // draw sampled frontiers
1379  for (int fid = 0; fid < frontier_list.size(); fid++)
1380  {
1381  int x = frontier_list[fid].position.x();
1382  int y = -frontier_list[fid].position.y();
1383  cv::circle(temp, cv::Point(x, y), 1, CV_RGB(255, 0, 255));
1384  }
1385  // draw invalid frontiers
1386  for (auto it = task_maybe_invalid.begin(); it != task_maybe_invalid.end(); it++)
1387  {
1388  int x = it->x();
1389  int y = -it->y();
1390  cv::circle(temp, cv::Point(x, y), 1, CV_RGB(0, 255, 255));
1391  }
1392  // for (int fid = 0; fid < task_maybe_invalid.size(); fid++)
1393  // {
1394  // int x = task_maybe_invalid[fid].x();
1395  // int y = -task_maybe_invalid[fid].y();
1396  // cv::circle(temp, cv::Point(x, y), 1, CV_RGB(0, 255, 255));
1397  // }
1398  // save file
1399  char output_path[200];
1400  sprintf(output_path, "result/sampled_frontier_%d.png", g_plan_iteration);
1401  cv::imwrite(output_path, temp);
1402  }
1403 
1404  cv::Mat locaMap = computeLoationMap(); // known region structure
1405  // generate NBVs
1406  while (!frontier_list.empty())
1407  {
1408  FrontierElement current_frontier = frontier_list.front();
1409  cv::Point fp(current_frontier.position.x(), -current_frontier.position.y());
1410  vector<cv::Point> vps;
1411  const int rangeMIN = 10; // pixel
1412  const int rangeMAX = 40; // pixel
1413  int delta = rangeMAX;
1414  int beg_r = fp.y - delta < 0 ? 0 : fp.y - delta;
1415  int end_r = fp.y + delta > map_rows - 1 ? map_rows - 1 : fp.y + delta;
1416  int beg_c = fp.x - delta < 0 ? 0 : fp.x - delta;
1417  int end_c = fp.x + delta > map_cols - 1 ? map_cols - 1 : fp.x + delta;
1418  for (int r = beg_r; r <= end_r; r++)
1419  {
1420  for (int c = beg_c; c <= end_c; c++)
1421  {
1422  //cv::circle(visual, cv::Point(c, r), 2, CV_RGB(0, 255, 0)); // test
1423  if (m_p_de->m_recon2D.m_cellmap[r][c].isScanned && m_p_de->m_recon2D.m_cellmap[r][c].isFree) // valid position in domain
1424  {
1425  if ((double)rand() / RAND_MAX < 0.995) // random sample, 0.9 is ok but not efficient.
1426  continue;
1427  double eDistance = m_metric.get_euclidean_distance(cv::Point(c, r), fp);
1428  if (eDistance > rangeMIN) // outside min of scan range
1429  {
1430  if (checkViewRayValidness(cv::Point(c, r), fp))
1431  {
1432  //cv::circle(visual, cv::Point(c, r), 2, CV_RGB(255, 0, 255)); // test
1433  vps.push_back(cv::Point(c, r));
1434  }
1435  }
1436  }
1437  }
1438  }
1439  // sample
1440  const int numMax = 10;
1441  if (vps.size() > numMax)
1442  {
1443  int num = vps.size() - numMax;
1444  for (int i = 0; i < num; i++)
1445  {
1446  int idx = (int)floor(rand() % (vps.size() - 1));
1447  vps.erase(vps.begin() + idx);
1448  }
1449  }
1450  // score these candidate positions
1451  vector<double> scores;
1452  scores.resize(vps.size());
1453  for (int idx = 0; idx < scores.size(); idx++)
1454  {
1455  scores[idx] = locaMap.ptr<uchar>(vps[idx].y)[vps[idx].x];
1456  }
1457  double max_score = -1;
1458  double max_index = -1;
1459  for (int idx = 0; idx < vps.size(); idx++) // select the best
1460  {
1461  if (max_score < scores[idx])
1462  {
1463  max_score = scores[idx];
1464  max_index = idx;
1465  }
1466  }
1467  // valid check
1468  if (max_index == -1)
1469  {
1470  task_maybe_invalid.insert(Point_2(frontier_list.begin()->position.x() + (double)rand() / RAND_MAX / 10, frontier_list.begin()->position.y()));
1471  frontier_list.erase(frontier_list.begin());
1472  //cout << "no valid views, continue to next frontier..." << endl;
1473  continue;
1474  }
1475  //cerr<<"task_maybe_invalid.size() = "<<task_maybe_invalid.size()<<endl;
1476  //getchar();getchar();getchar();
1477  // save NBV
1478  cv::Point nbp = vps[max_index];
1479  Eigen::Vector2d base(0.0, 1.0); // se2 coor
1480  Eigen::Vector2d dire(fp.x - nbp.x, -fp.y + nbp.y); // se2 coor
1481  dire.normalize();
1482  double theta = acos(base.dot(dire) / (dire.norm() * base.norm()));
1483  if (dire.x() > 0)
1484  theta = -theta;
1485  NextBestView nbv(iro::SE2(nbp.x, -nbp.y, theta), 1, nbvs.size());
1486  //cerr << "theta = " << theta << endl; // test
1487  nbvs.push_back(nbv);
1488  //cout << "generated a view stored." << endl;
1489  // delete covered frontiers // frustum and visibility check. without openmp. better than with openmp, why?
1490  {
1491  // erase this frontier
1492  frontier_list.erase(frontier_list.begin());
1493  // erase covered frontiers by this view
1494  vector<int> dlt_indexes;
1495  vector<cv::Point> nbv_frustum = get_frustum_contuor(nbv.pose);
1496  for (int i = 0; i < frontier_list.size(); i++)
1497  {
1498  // check view frustum
1499  if (cv::pointPolygonTest(nbv_frustum, cv::Point((int)round(frontier_list[i].position.x()), -(int)round(frontier_list[i].position.y())), false) > 0) // in field of frustum
1500  {
1501  // check visiability
1502  cv::Point beg((int)round(nbv.pose.translation().x()), -(int)round(nbv.pose.translation().y()));
1503  cv::Point end((int)round(frontier_list[i].position.x()), -(int)round(frontier_list[i].position.y()));
1504  // if visiable, delete the covered frontiers from queue
1505  if (checkViewRayValidness(beg, end))
1506  dlt_indexes.push_back(i);
1507  }
1508  }
1509  if (!dlt_indexes.empty())
1510  {
1511  for (int i = dlt_indexes.size() - 1; i >= 0; i--)
1512  {
1513  frontier_list.erase(frontier_list.begin() + dlt_indexes[i]);
1514  }
1515  }
1516  }
1517  }
1518  // end.
1519  return nbvs;
1520 }
1521 
1522 // tasks func
1523 vector<NextBestView> Navigation::compute_frontier_tasks(Polygon_2 boundary, vector<Polygon_2> holes)
1524 {
1525  cerr << "computing views for frontiers...";
1526  // set up
1527  vector<NextBestView> valid_frontier_nbvs;
1528  //valid_frontier_nbvs.clear();
1529  vector<int> task_index_in_valid_frontier_nbvs;
1530  //task_index_in_valid_frontier_nbvs.clear();
1531  vector<int> frontier_task_indexes; // frontier task indexes
1532  int frt_num = 0; // number of frontier tasks
1533  preprocess_frontiers(boundary, holes); // preprocess to m_frontierList
1534  for (int i = 0; i < m_frontierList.size(); i++)
1535  frontier_task_indexes.push_back(i);
1536  frt_num = frontier_task_indexes.size();
1537  vector<NextBestView> frontier_task_nbvs;
1538  // NBV
1539  vector<NextBestView> nbvs_frt; // 2018-11-05.
1540  //if (scene_name == "wallrect" && holes.empty()) // test in wall_rectangle scene
1541  // nbvs_frt = generateViewsFrontiersNoneObstacle(true, 0.5);
1542  //else
1543  // nbvs_frt = generateViewsFrontiers(true, 0.5);
1544  nbvs_frt = generateViewsFrontiers(true, 0.5);
1545  valid_frontier_nbvs = nbvs_frt;
1546  // final tasks // intersect; whats meannig of these code???
1547  for (int i = 0; i < nbvs_frt.size(); i++)
1548  {
1549  bool valid = false;
1550  for (int j = 0; j < frontier_task_indexes.size(); j++)
1551  {
1552  if (nbvs_frt[i].index == frontier_task_indexes[j])
1553  {
1554  valid = true;
1555  break;
1556  }
1557  }
1558  if (valid)
1559  {
1560  frontier_task_nbvs.push_back(nbvs_frt[i]);
1561  task_index_in_valid_frontier_nbvs.push_back(i);
1562  }
1563  }
1564  frt_num = frontier_task_nbvs.size();
1565 /*
1566  // test
1567  if (frontier_task_nbvs.size() == 0)
1568  {
1569  cout << m_frontierList.size() << " origin valid frontiers remain." << endl;
1570  cout << nbvs_frt.size() << " views for frontiers." << endl;
1571  cout << frontier_task_nbvs.size() << " views for frontiers remain." << endl;
1572  }
1573 //*/
1574  cerr<<"done."<<endl;
1575  return frontier_task_nbvs;
1576 }
1577 
1578 // exe func: return nbvs, not finish. todo: NBV for quality.
1579 vector<NextBestView> Navigation::extractQualityTaskViews(Polygon_2 boundary, vector<Polygon_2> holes)
1580 {
1581  // set up
1582  m_valid_object_nbvs.clear();
1583  m_task_index_in_valid_object_nbvs.clear();
1584  vector<NextBestView> object_task_nbvs; // results return
1585 /*
1586  // set up ???
1587  int obj_num = 0; // number of object tasks
1588  // extract initial task targets
1589  vector<Eigen::Vector3i> initial_targets = findQualityTargets();
1590  // check quit impossible positions // need to check if correct
1591  vector<Eigen::Vector3i> targets;
1592  for (int i = 0; i < initial_targets.size(); i++)
1593  {
1594  // target task in se2 coordinate
1595  Eigen::Vector2d targ(initial_targets[i][1], -initial_targets[i][0]);
1596  // check
1597  bool visible = true;
1598  for (auto unvisible_task = task_that_should_be_scan.begin(); unvisible_task != task_that_should_be_scan.end(); unvisible_task++)
1599  {
1600  // impoosible task in se2 coordinate
1601  Eigen::Vector2d hard(unvisible_task->x(), unvisible_task->y());
1602  if ((targ - hard).norm() <= offset_size)
1603  {
1604  visible = false;
1605  break;
1606  }
1607  }
1608  if (!visible)
1609  continue;
1610  targets.push_back(initial_targets[i]);
1611  }
1612  // compute NBV to cover targets
1613  vector<NextBestView> nbvs_obj = computeViewsForTargets(targets, boundary, holes);
1614  m_valid_object_nbvs = nbvs_obj;
1615  object_task_nbvs = nbvs_obj;
1616  obj_num = object_task_nbvs.size();
1617  // sort and filtering
1618  {
1619  // sort
1620  InfoGain ig;
1621  map<float, NextBestView> sorted_nbvs;
1622  for (int i = 0; i < object_task_nbvs.size(); i++)
1623  {
1624  float gain = ig.ComputeInfoGain_Target(object_task_nbvs[i]);
1625  map<float, NextBestView>::iterator finder = sorted_nbvs.find(gain);
1626  while (finder != sorted_nbvs.end())
1627  {
1628  //cerr << "repeat gain..." << endl;
1629  //getchar();
1630  // add noise.
1631  float noise = (float)rand() / RAND_MAX * 0.0001;
1632  gain += noise;
1633  finder = sorted_nbvs.find(gain);
1634  }
1635  sorted_nbvs.insert(pair<float, NextBestView>(gain, object_task_nbvs[i]));
1636  }
1638  //{
1639  // cerr << "info_gain_thresh = " << info_gain_thresh << endl;
1640  // for (auto it = sorted_nbvs.begin(); it != sorted_nbvs.end(); it++)
1641  // {
1642  // cerr << "sorted gain = " << it->first << endl;
1643  // }
1644  // cerr << "continue?" << endl;
1645  // getchar();
1646  //}
1647  // save
1648  {
1649  object_task_nbvs.clear();
1650  vector<NextBestView> temp;
1651  for (auto it = sorted_nbvs.begin(); it != sorted_nbvs.end(); it++)
1652  {
1653  if (it->first < info_gain_thresh) continue;
1654  //cerr << "pushed info gain " << it->first << endl;
1655  temp.push_back(it->second);
1656  }
1657  for (int idx = temp.size() - 1; idx >= 0; idx--)
1658  {
1659  //cerr << "gain: " << ig.ComputeInfoGain_Target(temp[idx]) << endl;
1660  object_task_nbvs.push_back(temp[idx]);
1661  }
1662  }
1663  }
1664 //*/
1665  // end.
1666  return object_task_nbvs;
1667 }
1668 
1669 // extract tasks, not finish
1670 bool Navigation::extractTasks(Polygon_2 boundary, vector<Polygon_2> holes, vector<ScanningTask> & tasks)
1671 {
1672  // frontiers tasks
1673  vector<NextBestView> frontier_tasks = compute_frontier_tasks(boundary, holes);
1674  int frt_num = frontier_tasks.size();
1675  vector<double> frontier_task_distances(frontier_tasks.size());
1676  // priority of frontier tasks. less distance higher priority.
1677  {
1678  // load robot poses
1679  vector<Point_2> rbtPoses(m_robot_sites);
1680  // compute distance to robots
1681  for (int fid = 0; fid < frontier_tasks.size(); fid++)
1682  {
1683  // check nan
1684  if (__isnan(frontier_tasks[fid].pose.translation().x()) || __isnan(frontier_tasks[fid].pose.translation().y()))
1685  {
1686  cerr<<"frontier_tasks["<<fid<<"] nan: "<<frontier_tasks[fid].pose.translation().transpose()<<endl;
1687  getchar(); getchar(); getchar();
1688  continue;
1689  }
1690  // compute distance from closest robot
1691  double min_d = 999999999;
1692  {
1693  vector<Point_2> targets;
1694  for (int rid = 0; rid < rbtPoses.size(); rid++)
1695  targets.push_back(rbtPoses[rid]);
1696  Point_2 taskP(frontier_tasks[fid].pose.translation().x(), frontier_tasks[fid].pose.translation().y());
1697  vector<double> distances = m_metric.getGeodesicDistances(taskP, targets);
1698  for (int rid = 0; rid < distances.size(); rid++)
1699  if (min_d > distances[rid])
1700  min_d = distances[rid];
1701  }
1702  frontier_task_distances[fid] = min_d;
1703  }
1704  }
1706  //if (frontier_tasks.size() == 0) confidence_threshold = 1.5;
1707  // object tasks. modify
1708  vector<NextBestView> object_tasks = extractQualityTaskViews(boundary, holes);
1709  int ldmk_num = object_tasks.size();
1710 
1712  //if (object_tasks.size() == 0)
1713  //{
1714  // confidence_threshold = 2.5;
1715  // object_tasks = extractQualityTaskViews(boundary, holes);
1716  // ldmk_num = object_tasks.size();
1717  //}
1718 
1719  // confidence_threshold fixed.
1720  object_tasks = extractQualityTaskViews(boundary, holes);
1721  ldmk_num = object_tasks.size();
1722 
1724  //object_tasks.clear();
1725  //ldmk_num = 0;
1726 
1727  // tasks conclusion // ScanningTask data structure
1728  int task_num;
1729  {
1730  // task explore
1731  for (int i = 0; i < frontier_tasks.size(); i++) // exploration tasks
1732  tasks.push_back(ScanningTask(1, frontier_tasks[i]));
1733  // task number control
1734  {
1735  if (tasks.size() > m_max_task_num)
1736  {
1737  int d_num = tasks.size() - m_max_task_num;
1738  for (int i = 0; i < d_num; i++)
1739  {
1740  // delete farthest
1741  {
1742  double maxValue = 0;
1743  int maxID = -1;
1744  for (int index = 0; index < frontier_task_distances.size(); index++)
1745  {
1746  if (frontier_task_distances[index] > maxValue)
1747  {
1748  maxValue = frontier_task_distances[index];
1749  maxID = index;
1750  }
1751  }
1752  if (maxID == -1)
1753  {
1754  cerr << "error, ID = -1" << endl; getchar(); getchar(); getchar();
1755  }
1756  tasks.erase(tasks.begin() + maxID);
1757  frontier_task_distances.erase(frontier_task_distances.begin() + maxID);
1758  }
1759  }
1760  }
1761  }
1762  //{ // test
1763  // vector<Point_2> rbtPoses;
1764  // loadRobotSites(rbtPoses);
1765  // for (int index = 0; index < rbtPoses.size(); index++)
1766  // {
1767  // cerr << "plot(" << rbtPoses[index].x() << ", " << rbtPoses[index].y() << ", 'bx'); hold on;" << endl;
1768  // }
1769  // for (int index = 0; index < frontier_tasks.size(); index++)
1770  // {
1771  // cerr << "plot(" << frontier_tasks[index].pose.translation().x() << ", " << frontier_tasks[index].pose.translation().y() << ", 'go'); hold on;" << endl;
1772  // }
1773  // for (int index = 0; index < tasks.size(); index++)
1774  // {
1775  // cerr << "plot(" << tasks[index].view.pose.translation().x() << ", " << tasks[index].view.pose.translation().y() << ", 'ro'); hold on;" << endl;
1776  // }
1777  // getchar(); getchar(); getchar();
1778  //}
1779  // task quality
1780  if (tasks.size() < m_max_task_num)
1781  {
1782  for (int i = 0; i < object_tasks.size(); i++) // quality tasks
1783  tasks.push_back(ScanningTask(0, object_tasks[i]));
1784  }
1785  // task number control
1786  {
1787  if (tasks.size() > m_max_task_num)
1788  {
1789  int d_num = tasks.size() - m_max_task_num;
1790  for (int i = 0; i < d_num; i++)
1791  tasks.pop_back();
1792  }
1793  }
1794  task_num = tasks.size();
1795  cerr << frt_num << " views for exploration, " << ldmk_num << " views for quality" << endl;
1796  cerr << task_num << " task views selected. " << endl;
1797  }
1798 //*/
1799  return true;
1800 }
1801 
1802 // not finished...
1803 void Navigation::scanFinished()
1804 {
1805  // todo...
1806  printf("scan finished.\n");
1807  exit(0);
1808  return;
1809 }
1810 
1811 // TSP.
1812 vector<int> Navigation::TSP_path(vector<Point_2> points, vector<vector<double>> weights)
1813 {
1814  // set up
1815  vector<int> result;
1816  // few points
1817  if (points.size() <= 3)
1818  {
1819  if (points.size() == 1)
1820  {
1821  result.push_back(0);
1822  }
1823  else if (points.size() == 2)
1824  {
1825  result.push_back(0);
1826  result.push_back(1);
1827  }
1828  else // 3
1829  {
1830  result.push_back(0);
1831  if (weights[0][1] < weights[0][2])
1832  {
1833  result.push_back(1);
1834  result.push_back(2);
1835  }
1836  else
1837  {
1838  result.push_back(2);
1839  result.push_back(1);
1840  }
1841  }
1842  // return
1843  return result;
1844  }
1845  // set up input for TSP solver
1846  vector<Eigen::Vector2d> nodes;
1847  for (int i = 0; i < points.size(); i++)
1848  nodes.push_back(Eigen::Vector2d(points[i].x(), points[i].y()));
1849  // TSP solver
1850  TSP tsp(nodes, weights);
1851 
1852  // Christofides Algorithm
1853  {
1854  // Find a MST T in graph G
1855  tsp.findMST_old();
1856  // Find a minimum weighted matching M for odd vertices in T
1857  tsp.perfect_matching();
1858  // Find the node that leads to the best path - - -
1859  // Create array of thread objects
1860  MyThread threads[NUM_THREADS];
1861  int best = INT_MAX;
1862  int bestIndex;
1863  int stop_here = NUM_THREADS;
1864  // Amount to increment starting node by each time
1865  int increment = 1; // by 1 if n < 1040
1866  int n = tsp.get_size();
1867  if (n >= 600 && n < 1040)
1868  increment = 3;
1869  else if (n >= 1040 && n < 1800)
1870  increment = 8;
1871  else if (n >= 1800 && n < 3205)
1872  increment = 25; // ~ 220s @ 3200
1873  else if (n >= 3205 && n < 4005)
1874  increment = 50; // ~ 230s @ 4000
1875  else if (n >= 4005 && n < 5005)
1876  increment = 120; // ~ 200 @ 5000
1877  else if (n >= 5005 && n < 6500)
1878  increment = 250; // ~ 220s @ 6447
1879  else if (n >= 6500)
1880  increment = 500;
1881  int remaining = n;
1882  // Start at node zero
1883  int node = 0;
1884  // Count to get thread ids
1885  int count = 0;
1886  while (remaining >= increment)
1887  {
1888  // Keep track iteration when last node will be reached
1889  if (remaining < (NUM_THREADS * increment))
1890  {
1891  // each iteration advances NUM_THREADS * increment nodes
1892  stop_here = remaining / increment;
1893  }
1894  for (long t = 0; t < stop_here; t++)
1895  {
1896  //cout << "Thread " << count << " starting at node " << node << endl;
1897  threads[t].start_node = node;
1898  threads[t].my_id = count;
1899  threads[t].mytsp = &tsp;
1900  threads[t].start();
1901  node += increment;
1902  count++;
1903  }
1904  // Wait for all the threads
1905  for (long t = 0; t < stop_here; t++)
1906  {
1907  threads[t].join();
1908  }
1909  remaining -= (stop_here * increment);
1910  }
1911  // Loop through each index used and find shortest path
1912  for (long t = 0; t < count; t++) {
1913  if (tsp.path_vals[t][1] < best) {
1914  bestIndex = tsp.path_vals[t][0];
1915  best = tsp.path_vals[t][1];
1916  }
1917  }
1918  // Store best path
1919  tsp.create_tour(bestIndex);
1920  tsp.make_shorter();
1921  tsp.make_shorter();
1922  tsp.make_shorter();
1923  tsp.make_shorter();
1924  tsp.make_shorter();
1925  }
1926  // path // remove the first/last path, depend on which it longer
1927  int start_id = -1;
1928  bool posetive_sequence = true;
1929  // find start id and sequence direction
1930  for (int i = 0; i < tsp.circuit.size(); i++)
1931  {
1932  if (tsp.circuit[i] == 0)
1933  {
1934  start_id = i;
1935  int ni = (i + 1) % tsp.circuit.size();
1936  int li = (i - 1 + tsp.circuit.size()) % tsp.circuit.size();
1937  //cerr << "weights[0][tsp.circuit[ni]] = " << weights[0][tsp.circuit[ni]] << endl;
1938  //cerr << "weights[0][tsp.circuit[li]] = " << weights[0][tsp.circuit[li]] << endl;
1939  if (weights[0][tsp.circuit[ni]] < weights[0][tsp.circuit[li]])
1940  posetive_sequence = true;
1941  else
1942  posetive_sequence = false;
1943  break;
1944  }
1945  }
1946  for (int counter = 0; counter < tsp.circuit.size(); counter++)
1947  {
1948  if (posetive_sequence)
1949  {
1950  int id = (start_id + counter) % tsp.circuit.size();
1951  result.push_back(tsp.circuit[id]);
1952  }
1953  else
1954  {
1955  int id = (start_id - counter + tsp.circuit.size()) % tsp.circuit.size();
1956  result.push_back(tsp.circuit[id]);
1957  }
1958  }
1959  return result;
1960 }
1961 
1962 // OMT with TSP, not finish
1963 void Navigation::OMT_TSP()
1964 {
1965 
1966 // : Generate Domain. outer boundary and inner holes.
1967  Polygon_2 boundary; // domain outer boundary
1968  vector<Polygon_2> holes; // domain invalid holes
1969  vector<Polygon_2> origin_holes; // origin domain incalid holes
1970  {
1971  robotMoveDomainProcess(boundary, holes, origin_holes); // generate robot move domain
1972  //double t_beg = clock(); // timing
1973  // domain CDT
1974  CDT cdt;
1975  vector<ScanningTask> temp; // useless
1976  generateMoveDomainCDT(boundary, holes, origin_holes, temp, cdt);
1977  // save mesh for geodesic computation
1978  saveCDT_geodesic(cdt);
1979  m_metric.get_geodesic_distance_fast_initialization(); // init geodesic
1980  //double t_end = clock(); // timing
1981  //cerr << "- CDT timing " << t_end - t_beg << " ms" << endl;
1982  }
1983  //double t1 = clock(); // timing
1984 
1985 // : Generate Views.
1986  vector<ScanningTask> tasks; // scan tasks
1987  vector<Point_2> frontiers; // frontier
1988  {
1989  // frontiers
1990  frontiers = load_and_check_frontiers(boundary, holes); // frontier positions, global variable
1991  // tasks
1992  extractTasks(boundary, holes, tasks);
1993  // check if scan finished
1994  if (tasks.size() == 0)
1995  {
1996  scanFinished();
1997  }
1998  // update domain cdt sites with task_sites
1999  //double t_beg = clock(); // timing
2000  CDT cdt;
2001  generateMoveDomainCDT(boundary, holes, origin_holes, tasks, cdt);
2002  // save mesh for geodesic computation
2003  saveCDT_geodesic(cdt); // save mesh to compute geodesic
2004  m_metric.get_geodesic_distance_fast_initialization(); // init geodesic
2005  //double t_end = clock(); // timing
2006  //cerr << "- CDT timing " << t_end - t_beg << " ms" << endl;
2007  }
2008 
2009 // : OMT Task Assignment
2010  vector<vector<NextBestView>> assigned_tasks; // assignment: output from assignment method
2011  // discrete omt
2012  {
2013  // domain
2014  CDT cdt; // input
2015  generateMoveDomainCDT(boundary, holes, origin_holes, tasks, cdt);
2016  saveCDT_geodesic(cdt); // save mesh to compute geodesic
2017  m_metric.get_geodesic_distance_fast_initialization(); // init geodesic
2018  // robot pose input
2019  vector<Eigen::Vector2d> robots(m_robot_sites.size()); // input
2020  for (int rid = 0; rid < m_robot_sites.size(); rid++)
2021  {
2022  robots[rid] = Eigen::Vector2d(m_robot_sites[rid].x(), m_robot_sites[rid].y());
2023  //cerr<<"robots["<<rid<<"] "<<robots[rid].transpose()<<endl;
2024  }
2025  // task view input
2026  vector<Eigen::Vector2d> task_views(tasks.size()); // input
2027  for (int vid = 0; vid < tasks.size(); vid++)
2028  task_views[vid] = Eigen::Vector2d(
2029  tasks[vid].view.pose.translation().x(),
2030  tasks[vid].view.pose.translation().y()
2031  );
2032  // omt solver
2033  DiscreteSolver d_solver(*m_p_de, cdt, m_metric, robots, task_views, compactParam);
2034  //DiscreteSolver d_solver(*m_p_de, cdt, m_metric, m_metric.GeodesicDistances, robots, task_views, compactParam);
2035  d_solver.SolveOMT();
2036  vector<vector<int>> assignment = d_solver.getSolution();
2037  // save the assignment
2038  assigned_tasks.resize(assignment.size());
2039  for (int rid = 0; rid < assignment.size(); rid++)
2040  {
2041  assigned_tasks[rid].resize(assignment[rid].size());
2042  for (int tid = 0; tid < assignment[rid].size(); tid++)
2043  {
2044  int vid = assignment[rid][tid];
2045  assigned_tasks[rid][tid] = NextBestView(tasks[vid].view);
2046  }
2047  }
2048  }
2049 
2050 // : TSP Order Planning.
2051  {
2052  // setup
2053  m_robot_move_views.clear();
2054  m_robot_move_views.resize(rbt_num);
2055  for (int rid = 0; rid < rbt_num; rid++)
2056  m_robot_move_views[rid].clear();
2057  // solve TSP (scan order of tasks) for each robot
2058  for (int rid = 0; rid < rbt_num; rid++)
2059  {
2060  // nodes
2061  vector<Point_2> nodes; // index == assigned_tasks[rid].index+1
2062  nodes.push_back(Point_2(m_robot_sites[rid].x(), m_robot_sites[rid].y()));
2063  for (int tid = 0; tid < assigned_tasks[rid].size(); tid++)
2064  nodes.push_back(Point_2(assigned_tasks[rid][tid].pose.translation().x(), assigned_tasks[rid][tid].pose.translation().y()));
2065  // weights
2066  vector<vector<double>> weights; // weights
2067  weights.resize(nodes.size());
2068  for (int i = 0; i < nodes.size(); i++)
2069  {
2070  weights[i].resize(nodes.size());
2071  }
2072  // compute weights batch & paralell.
2073  {
2074  // weights between i j
2075  vector<vector<double>> wij(nodes.size());
2076  for (int i = 0; i < wij.size(); i++)
2077  wij[i].resize(nodes.size());
2078 #pragma omp parallel for num_threads(8)
2079  //for (int i = 0; i < nodes.size(); i++)
2080  for (int i = 0; i < nodes.size() - 1; i++)
2081  {
2082  vector<Point_2> targets;
2083  for (int j = i + 1; j < nodes.size(); j++)
2084  targets.push_back(nodes[j]);
2085  vector<double> cDistances = m_metric.getGeodesicDistances(nodes[i], targets); // compute geodesic
2086  for (int tid = 0; tid < targets.size(); tid++)
2087  {
2088  int j = tid + i + 1;
2089  wij[i][j] = cDistances[tid];
2090  wij[j][i] = cDistances[tid];
2091  }
2092  }
2093  // weights
2094  //for (int i = 0; i < nodes.size(); i++)
2095  for (int i = 0; i < nodes.size() - 1; i++)
2096  {
2097  for (int j = i + 1; j < nodes.size(); j++)
2098  {
2099  // distance weight
2100  double distance_weight = wij[i][j];
2101  // angle weight
2102  double angle_weight(0);
2103  {
2104  // theta of node i
2105  double theta1;
2106  if (i == 0) // i from 0 to size-1, so it may be the first node
2107  {
2108  theta1 = m_p_de->m_pose2d[rid].rotation().angle();
2109  }
2110  else
2111  {
2112  theta1 = assigned_tasks[rid][i - 1].pose.rotation().angle();
2113  }
2114  if (theta1 < 0) theta1 = 2 * PI + theta1;
2115  // theta of node j
2116  double theta2 = assigned_tasks[rid][j - 1].pose.rotation().angle();
2117  if (theta2 < 0) theta2 = 2 * PI + theta2;
2118  double delta = fabs(theta1 - theta2) < 2 * PI - fabs(theta1 - theta2) ? fabs(theta1 - theta2) : 2 * PI - fabs(theta1 - theta2);
2119  angle_weight = delta / PI;
2120  // check
2121  //if (_isnan(angle_weight))
2122  if (__isnan(angle_weight))
2123  {
2124  cerr << "angle_weight = " << angle_weight << " is nan" << endl;
2125  cerr << "theta1 = " << theta1 << endl;
2126  cerr << "theta2 = " << theta2 << endl;
2127  cerr << "delta = " << delta << endl;
2128  exit(-1);
2129  }
2130  }
2131  // final weight
2132  double final_weight = max(distance_weight, angle_weight);
2133  weights[i][j] = weights[j][i] = final_weight;
2134  }
2135  }
2136  // release memory
2137  for (int i = 0; i < wij.size(); i++)
2138  vector<double>().swap(wij[i]);
2139  wij.clear();
2140  vector<vector<double>>().swap(wij);
2141  }
2142  // path computation
2143  vector<int> point_path;
2144  if (nodes.size() == 1)
2145  {
2146  point_path.push_back(0); // only one noed
2147  }
2148  else
2149  {
2150  point_path = TSP_path(nodes, weights); // TSP solver
2151  }
2152  // store path
2153  for (int i = 0; i < point_path.size(); i++)
2154  {
2155  if (point_path[i] == 0)
2156  {
2157  // robot current pose
2158  m_robot_move_views[rid].push_back(m_p_de->m_pose2d[rid]);
2159  }
2160  else
2161  {
2162  // robot next best view poses
2163  int tid = point_path[i] - 1;
2164  m_robot_move_views[rid].push_back(iro::SE2(assigned_tasks[rid][tid].pose.translation().x(), assigned_tasks[rid][tid].pose.translation().y(), assigned_tasks[rid][tid].pose.rotation().angle()));
2165  }
2166  }
2167  // release memory
2168  for (int i = 0; i < weights.size(); i++)
2169  vector<double>().swap(weights[i]);
2170  weights.clear();
2171  vector<vector<double>>().swap(weights);
2172  // check
2173  if (m_robot_move_views[rid].size() != point_path.size())
2174  {
2175  cerr << "size error: " << m_robot_move_views[rid].size() << ", " << point_path.size() << endl;
2176  getchar();
2177  }
2178  }
2179  }
2180  // timing
2181 /*
2182  // test
2183  {
2184  cv::Mat statement = m_p_de->visCellMap();
2185  for (int rid = 0; rid < rbt_num; ++rid)
2186  {
2187  for (int vid = 0; vid < m_robot_move_views[rid].size(); ++vid)
2188  {
2189  cv::circle(statement, cv::Point((int)m_robot_move_views[rid][vid].translation().x(), -(int)m_robot_move_views[rid][vid].translation().y()), 3, CV_RGB(0, 0, 255));
2190  if (vid>0)
2191  {
2192  cv::line(statement,
2193  cv::Point((int)m_robot_move_views[rid][vid].translation().x(), -(int)m_robot_move_views[rid][vid].translation().y()),
2194  cv::Point((int)m_robot_move_views[rid][vid-1].translation().x(), -(int)m_robot_move_views[rid][vid-1].translation().y()),
2195  CV_RGB(0, 0, 255));
2196  }
2197  }
2198  }
2199  cv::imshow("statement", statement);
2200  cv::waitKey(0);
2201  }
2202 //*/
2203  //done.
2204  return;
2205 }
2206 
2207 /*
2208 // check trojectories
2209 void Navigation::checkTrojectories()
2210 {
2211  // check: remove invalid point
2212  for (int rid = 0; rid < rbt_num; rid++)
2213  {
2214  // detect invalid
2215  vector<int> idxs;
2216  for (int pid = 0; pid < g_rbtTrajectories[rid].size(); pid++)
2217  {
2218  if (__isnan(g_rbtTrajectories[rid][pid].x()))
2219  {
2220  cout << "g_rbtTrajectories node nan..." << endl; getchar(); getchar();
2221  idxs.push_back(pid);
2222  continue;
2223  }
2224  if (g_rbtTrajectories[rid][pid].x() <= 0 || g_rbtTrajectories[rid][pid].y() >= 0)
2225  {
2226  cout << "g_rbtTrajectories node out of boundary..." << endl; getchar(); getchar();
2227  idxs.push_back(pid);
2228  continue;
2229  }
2230  }
2231  // delete invalid
2232  for (int i = idxs.size() - 1; i >= 0; i--)
2233  {
2234  cout << "delete invalid trajectory node_" << idxs[i] << " " << (g_rbtTrajectories[rid].begin() + idxs[i])->x() << ", " << (g_rbtTrajectories[rid].begin() + idxs[i])->y() << endl;
2235  g_rbtTrajectories[rid].erase(g_rbtTrajectories[rid].begin() + idxs[i]);
2236  }
2237  }
2238  return;
2239 }
2240 //*/
2241 
2242 // path_occlusion_check
2243 bool Navigation::path_occlusion_check(cv::Mat background, cv::Point beg, cv::Point end)
2244 {
2245  Eigen::Vector2d b(beg.x, beg.y);
2246  Eigen::Vector2d e(end.x, end.y);
2247  if ((b - e).norm() < 1)
2248  {
2249  return false;
2250  }
2251  // offset
2252  cv::Mat bin_map;
2253  cv::threshold(background, bin_map, 0, 255, cv::THRESH_BINARY);
2254  dilate(bin_map, bin_map, cv::getStructuringElement(0, cv::Size(offset_size * 2, offset_size * 2))); // offset of obsticles, used to aviod collision between robot and object
2255  //cv::imshow("before reset", background);
2256  // reset background
2257  for (int r = 0; r < map_rows; r++)
2258  {
2259  for (int c = 0; c < map_cols; c++)
2260  {
2261  if (bin_map.ptr<uchar>(r)[c] == 0)
2262  background.ptr<uchar>(r)[c] = 0;
2263  else
2264  background.ptr<uchar>(r)[c] = 255;
2265  }
2266  }
2267  //cv::imshow("after reset", background);
2268  //cv::waitKey(0);
2269  // dda check
2270  bool occlusion = false;
2271  int dx = end.x - beg.x;
2272  int dy = end.y - beg.y;
2273  if (dx == 0){
2274  int beg_y = beg.y < end.y ? beg.y : end.y;
2275  int end_y = beg.y > end.y ? beg.y : end.y;
2276  for (int y = beg_y; y < end_y; y++)
2277  if (background.ptr<uchar>(y)[beg.x] == 255)
2278  {
2279  occlusion = true;
2280  break;
2281  }
2282  }
2283  else if (dy == 0){
2284  int beg_x = beg.x < end.x ? beg.x : end.x;
2285  int end_x = beg.x > end.x ? beg.x : end.x;
2286  for (int x = beg_x; x < end_x; x++)
2287  if (background.ptr<uchar>(beg.y)[x] == 255)
2288  {
2289  occlusion = true;
2290  break;
2291  }
2292  }
2293  else if (dx == 0 && dy == 0){}
2294  else {
2295  int MaxStep = abs(dx) > abs(dy) ? abs(dx) : abs(dy);
2296  double fXUnitLen = 1.0;
2297  double fYUnitLen = 1.0;
2298  fYUnitLen = (double)(dy) / (double)(MaxStep);
2299  fXUnitLen = (double)(dx) / (double)(MaxStep);
2300  double x = (double)(beg.x);
2301  double y = (double)(beg.y);
2302  // 循环步进
2303  for (long i = 1; i < MaxStep; i++) // '<' or '<=', not include end point
2304  {
2305  x = x + fXUnitLen;
2306  y = y + fYUnitLen;
2307  if (background.ptr<uchar>((int)round(y))[(int)round(x)] == 255)
2308  //if (background.ptr<uchar>((int)floor(y))[(int)floor(x)] == 255)
2309  {
2310  occlusion = true;
2311  break;
2312  }
2313  }
2314  }
2315  return occlusion;
2316 }
2317 
2318 // compute m_robot_move_nodes
2319 bool Navigation::compute_robot_move_nodes(vector<double> & distances)
2320 {
2321  // set up
2322  m_robot_move_nodes.clear();
2323  m_robot_move_nodes.resize(rbt_num);
2324  m_robot_move_nodes_nbv_sign.clear();
2325  m_robot_move_nodes_nbv_sign.resize(rbt_num);
2326  distances.clear();
2327  distances.resize(rbt_num);
2328  for (int rid = 0; rid < rbt_num; rid++)
2329  distances[rid] = 0;
2330 /*// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
2331  //todo: smooth path
2332  // smooth interpolation
2333  if (exe_count > 0)
2334  {
2335  for (int rid = 0; rid < rbt_num; rid++)
2336  {
2337  if (m_robot_move_views[rid].size() == 1)
2338  continue;
2339  if ((m_p_de->m_pose2d[rid].translation() - m_robot_move_views[rid][1].translation()).norm() < 0.01) // same pose
2340  {
2341  }
2342  else // need smooth
2343  {
2344  vector<Point_2> goedesics;
2345  m_metric.get_geodesic_distance_fast(Point_2(m_p_de->m_pose2d[rid].translation().x(), m_p_de->m_pose2d[rid].translation().y()), Point_2(m_robot_move_views[rid][1].translation().x(), m_robot_move_views[rid][1].translation().y()), goedesics);
2346  if (goedesics.size() >= 2)
2347  {
2348  iro::SE2 nextViewPose(goedesics[1].x(), goedesics[1].y(), m_robot_move_views[rid][1].rotation().angle());
2349  // interpolation view position init
2350  Eigen::Vector2d itpViewPosition = (m_p_de->m_pose2d[rid].translation() + nextViewPose.translation()) / 2;
2351  double scale = (m_p_de->m_pose2d[rid].translation() - nextViewPose.translation()).norm() / 2 / 2; // scale
2352  //if (scale > 3) scale = 3;
2353  Eigen::Vector2d base;
2354  // - base angle: use path direction angle
2355  if (g_rbtTrajectories[rid].size() >= 2)
2356  {
2357  int idx = g_rbtTrajectories[rid].size() - 1;
2358  Eigen::Vector2d lastNode(g_rbtTrajectories[rid][idx].x(), g_rbtTrajectories[rid][idx].y());
2359  Eigen::Vector2d llstNode(g_rbtTrajectories[rid][idx - 1].x(), g_rbtTrajectories[rid][idx - 1].y());
2360  base = lastNode - llstNode;
2361  base.normalize();
2362  base *= scale; // scale
2363  }
2364  // - base angle: use current view angle
2365  else
2366  {
2367  base = Eigen::Vector2d(0, 1);
2368  base *= scale; // scale
2369  double angle = m_p_de->m_pose2d[rid].rotation().angle();
2370  Eigen::Matrix2d rotation;
2371  rotation << cos(angle), -sin(angle),
2372  sin(angle), cos(angle);
2373  base = rotation*base;
2374  }
2375  // interpolation view poisition final
2376  itpViewPosition += base;
2377  // interpolation view direction
2378  double itpViewDirection = nextViewPose.rotation().angle(); //double direction = (m_p_de->m_pose2d[rid].rotation().angle() + nextViewPose.rotation().angle()) / 2;
2379  // save
2380  {
2381  vector<iro::SE2> passViews;
2382  passViews.push_back(m_robot_move_views[rid][0]);
2383  if (!_isnan(itpViewPosition.x())) passViews.push_back(iro::SE2(itpViewPosition.x(), itpViewPosition.y(), itpViewDirection));
2384  for (int idx = 1; idx < m_robot_move_views[rid].size(); idx++)
2385  passViews.push_back(m_robot_move_views[rid][idx]);
2386  m_robot_move_views[rid].clear();
2387  m_robot_move_views[rid] = passViews;
2388  }
2389  }
2390  }
2391  }
2392  }
2393 //*/// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
2394  // compute m_robot_move_nodes geodesic nodes
2395  for (int rid = 0; rid < rbt_num; rid++)
2396  {
2397  // only one node(nbv) in path
2398  if (m_robot_move_views[rid].size() == 1)
2399  {
2400  distances[rid] = 0.0;
2401  m_robot_move_nodes[rid].push_back(m_robot_move_views[rid][0]);
2402  m_robot_move_nodes_nbv_sign[rid].push_back(false); // this node is not nbv node
2403  continue;
2404  }
2405  // multi nodes(nbvs) in path
2406  for (int vid = 0; vid < m_robot_move_views[rid].size() - 1; vid++)
2407  {
2408  int nid = vid + 1;
2409  // check same position
2410  {
2411  Eigen::Vector2d beg(m_robot_move_views[rid][vid].translation().x(), m_robot_move_views[rid][vid].translation().y());
2412  Eigen::Vector2d end(m_robot_move_views[rid][nid].translation().x(), m_robot_move_views[rid][nid].translation().y());
2413  if ((beg - end).norm() < 0.001) // same position of two points
2414  {
2415  if (m_robot_move_nodes[rid].empty()) // first node
2416  {
2417  m_robot_move_nodes[rid].push_back(iro::SE2(m_robot_move_views[rid][vid].translation().x(), m_robot_move_views[rid][vid].translation().y(), m_robot_move_views[rid][vid].rotation().angle()));
2418  m_robot_move_nodes_nbv_sign[rid].push_back(true); // this node is nbv node
2419  m_robot_move_nodes[rid].push_back(iro::SE2(m_robot_move_views[rid][nid].translation().x(), m_robot_move_views[rid][nid].translation().y(), m_robot_move_views[rid][nid].rotation().angle()));
2420  m_robot_move_nodes_nbv_sign[rid].push_back(true); // this node is nbv node
2421  }
2422  else
2423  {
2424  if (!se2_equal(m_robot_move_nodes[rid].back(), m_robot_move_views[rid][vid])) // avoid repeated node
2425  {
2426  m_robot_move_nodes[rid].push_back(iro::SE2(m_robot_move_views[rid][vid].translation().x(), m_robot_move_views[rid][vid].translation().y(), m_robot_move_views[rid][vid].rotation().angle()));
2427  m_robot_move_nodes_nbv_sign[rid].push_back(true); // this node is nbv node
2428  }
2429  m_robot_move_nodes[rid].push_back(iro::SE2(m_robot_move_views[rid][nid].translation().x(), m_robot_move_views[rid][nid].translation().y(), m_robot_move_views[rid][nid].rotation().angle()));
2430  m_robot_move_nodes_nbv_sign[rid].push_back(true); // this node is nbv node
2431  }
2432  continue;
2433  }
2434  }
2435  // not same position
2436  // occlusion check
2437  cv::Point beg(round(m_robot_move_views[rid][vid].translation().x()), -round(m_robot_move_views[rid][vid].translation().y()));
2438  cv::Point end(round(m_robot_move_views[rid][nid].translation().x()), -round(m_robot_move_views[rid][nid].translation().y()));
2439  cv::Mat background(map_rows, map_cols, CV_8UC1); // background
2440  for (int r = 0; r < background.rows; r++)
2441  {
2442  for (int c = 0; c < background.cols; c++)
2443  {
2444  if (m_p_de->m_recon2D.m_cellmap[r][c].isScanned && m_p_de->m_recon2D.m_cellmap[r][c].isFree)
2445  background.ptr<uchar>(r)[c] = 0;
2446  else
2447  background.ptr<uchar>(r)[c] = 255;
2448  }
2449  }
2450  vector<Point_2> path; // path
2451  Point_2 p1(m_robot_move_views[rid][vid].translation().x(), m_robot_move_views[rid][vid].translation().y());
2452  Point_2 p2(m_robot_move_views[rid][nid].translation().x(), m_robot_move_views[rid][nid].translation().y());
2453  if (path_occlusion_check(background, beg, end)) // obsticle
2454  {
2455  distances[rid] += m_metric.get_geodesic_distance(p1, p2, path); // distance
2456  //cerr << "text(" << p2.x() << ", " << p2.y() << ", '_g_" << distances[rid] << "'); hold on;" << endl;
2457  }
2458  else // not
2459  {
2460  distances[rid] += m_metric.get_euclidean_distance(p1, p2, path); // distance
2461  //cerr << "text(" << p2.x() << ", " << p2.y() << ", '_e_" << distances[rid] << "'); hold on;" << endl;
2462  }
2463  for (int tid = 0; tid < path.size(); tid++)
2464  {
2465  if (m_robot_move_nodes[rid].empty()) // first node
2466  {
2467  m_robot_move_nodes[rid].push_back(iro::SE2(path[tid].x(), path[tid].y(), m_robot_move_views[rid][vid].rotation().angle()));
2468  m_robot_move_nodes_nbv_sign[rid].push_back(false); // this node is not nbv node
2469  }
2470  else
2471  {
2472  if (!points_equal(Point_2(m_robot_move_nodes[rid].back().translation().x(), m_robot_move_nodes[rid].back().translation().y()), path[tid])) // avoid repeated node
2473  {
2474  if (tid == 0) // this node is nbv node
2475  {
2476  m_robot_move_nodes[rid].push_back(iro::SE2(path[tid].x(), path[tid].y(), m_robot_move_views[rid][vid].rotation().angle()));
2477  m_robot_move_nodes_nbv_sign[rid].push_back(true);
2478  }
2479  else if (tid == path.size() - 1) // this node is nbv node
2480  {
2481  m_robot_move_nodes[rid].push_back(iro::SE2(path[tid].x(), path[tid].y(), m_robot_move_views[rid][nid].rotation().angle()));
2482  m_robot_move_nodes_nbv_sign[rid].push_back(true);
2483  }
2484  else // this node is not nbv node
2485  {
2486  //m_robot_move_nodes[rid].push_back(iro::SE2(path[tid].x(), path[tid].y(), m_robot_move_views[rid][nid].rotation().angle()));
2487  m_robot_move_nodes[rid].push_back(iro::SE2(path[tid].x(), path[tid].y(), empty_angle));
2488  m_robot_move_nodes_nbv_sign[rid].push_back(false);
2489  }
2490  }
2491  }
2492  }
2493  }
2494  // error check
2495  if (!se2_equal(m_robot_move_nodes[rid].back(), m_robot_move_views[rid].back()))
2496  {
2497  // error
2498  cerr << "error, end point not equal" << endl;
2499  //getchar();
2500  m_robot_move_nodes[rid].push_back(m_robot_move_views[rid].back());
2501  m_robot_move_nodes_nbv_sign[rid].push_back(true);
2502  }
2503  }
2504  return true;
2505 }
2506 
2507 // check Point_2 equal
2508 bool Navigation::points_equal(Point_2 p1, Point_2 p2)
2509 {
2510  Eigen::Vector2d pose1(p1.x(), p1.y());
2511  Eigen::Vector2d pose2(p2.x(), p2.y());
2512  if ((pose1 - pose2).norm() < 0.00001)
2513  return true;
2514  return false;
2515 }
2516 
2517 // check se2 equal
2518 bool Navigation::se2_equal(iro::SE2 p1, iro::SE2 p2)
2519 {
2520  Eigen::Vector3d pose1(p1.translation().x(), p1.translation().y(), p1.rotation().angle());
2521  Eigen::Vector3d pose2(p2.translation().x(), p2.translation().y(), p2.rotation().angle());
2522  if ((pose1 - pose2).norm() < 0.00001)
2523  return true;
2524  return false;
2525 }
2526 
2527 // uniform sample nodes from nodes inside length
2528 vector<iro::SE2> Navigation::uniformSampleWithNBVInfo(const int robot_id, const double step, const double length)
2529 {
2530  // no move path
2531  if (m_robot_move_nodes[robot_id].size() == 1)
2532  {
2533  vector<iro::SE2> samples;
2534  samples.push_back(m_robot_move_nodes[robot_id].front()); // push the first node, that is robot current pose
2535  return samples;
2536  }
2537  // set up
2538  vector<Eigen::Vector2d> curve;
2539  for (int i = 0; i < m_robot_move_nodes[robot_id].size(); i++)
2540  curve.push_back(Eigen::Vector2d(m_robot_move_nodes[robot_id][i].translation().x(), m_robot_move_nodes[robot_id][i].translation().y()));
2541  // curve length
2542  vector<double> accLen;
2543  accLen.push_back(.0f);
2544  for (int i = 1; i < curve.size(); i++)
2545  {
2546  auto l = (curve[i - 1] - curve[i]).norm();
2547  accLen.push_back(accLen.back() + l);
2548  if (accLen.back() > length) // length
2549  break;
2550  }
2551  int numSamples = (int)floor(accLen.back() / step) + 1;
2552  bool debug = false;
2553  // same position of all points
2554  if (accLen.back() < 0.001)
2555  {
2556  vector<iro::SE2> samples;
2557  for (int i = 0; i < m_robot_move_nodes[robot_id].size(); i++)
2558  {
2559  samples.push_back(m_robot_move_nodes[robot_id][i]); // push the first node, that is robot current pose
2560  }
2561  return samples; // force return!
2562 
2563  numSamples = m_robot_move_nodes[robot_id].size();
2564  debug = true;
2565  }
2566  // sample
2567  vector<iro::SE2> samples;
2568  samples.push_back(m_robot_move_nodes[robot_id].front()); // push the first node
2569  double t = 0;
2570  int left = 0, right = 1;
2571  for (int i = 1; i < numSamples; i++)
2572  {
2573  // time
2574  t = i * step;
2575  // move to the segment containing t
2576  // t \in [accLen[left], accLen[right])
2577  while (t >= accLen[right])
2578  {
2579  //if (sample_save_origin_node)
2580  {
2581  if (m_robot_move_nodes_nbv_sign[robot_id][right])
2582  {
2583  samples.push_back(m_robot_move_nodes[robot_id][right]); // push origin NBV node
2584  }
2585  }
2586  left++;
2587  right++;
2588  }
2589  // sample in the range of [left, right]
2590  auto a = t - accLen[left];
2591  auto b = accLen[right] - t;
2592  auto s = (a * curve[right] + b * curve[left]) / (a + b);
2593  //samples.push_back(iro::SE2(s.x(), s.y(), (m_robot_move_nodes[robot_id][left].rotation().angle() + m_robot_move_nodes[robot_id][right].rotation().angle()) / 2)); // push sampled node
2594  //samples.push_back(iro::SE2(s.x(), s.y(), compute_optimal_scan_direction(cv::Point(approx(s.x()), -approx(s.y()))))); // push sampled node with max_gain
2595  samples.push_back(iro::SE2(s.x(), s.y(), empty_angle)); /* push sampled node, angle = 0 */ // optimization after path optimization
2596  }
2597  // test
2598  {
2599  for (int i = 0; i < samples.size(); i++)
2600  {
2601  if ((int)round(samples[i].translation().x()) < 10 && -(int)round(samples[i].translation().y()) < 10)
2602  {
2603 
2604  cerr << "error uniform sampled a invalid se2" << endl;
2605 
2606  getchar();
2607  // reset
2608  samples.clear();
2609  for (int i = 0; i < m_robot_move_nodes[robot_id].size(); i++)
2610  {
2611  samples.push_back(m_robot_move_nodes[robot_id][i]); // push the first node, that is robot current pose
2612  }
2613  return samples; // force return!
2614  }
2615  }
2616  }
2617  // force debug // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
2618  if (samples.size() == 1 && m_robot_move_nodes[robot_id].size() > 1)
2619  {
2620  samples.clear();
2621  for (int i = 0; i < m_robot_move_nodes[robot_id].size(); i++)
2622  {
2623  samples.push_back(m_robot_move_nodes[robot_id][i]); // push the first node, that is robot current pose
2624  }
2625  return samples; // force return!
2626  }
2627  return samples;
2628 }
2629 
2630 // compute m_sync_move_paths, not finish
2631 bool Navigation::compute_sync_move_paths(vector<double> distances)
2632 {
2633  // set up
2634  m_sync_move_paths.clear();
2635  m_sync_move_paths.resize(rbt_num);
2636  // compute m_sync_move_paths
2637  double distance_step = g_distance_step/map_cellsize;
2638  double min_distance = DBL_MAX;
2639  for (int rid = 0; rid < rbt_num; rid++)
2640  {
2641  if (distances[rid] != 0 && min_distance > distances[rid])
2642  min_distance = distances[rid];
2643  }
2644  for (int rid = 0; rid < rbt_num; rid++)
2645  {
2646  m_sync_move_paths[rid] = uniformSampleWithNBVInfo(rid, distance_step, min_distance);
2647  // path optimization
2648  {
2649  // optimize local: cut to segments and optimize each segment independent
2650  int start_id = -1, end_id = -1;
2651  for (int nid = 0; nid < m_sync_move_paths[rid].size(); nid++)
2652  {
2653  if (fabs(m_sync_move_paths[rid][nid].rotation().angle() - empty_angle) > 0.01) // nbv node
2654  {
2655  if (start_id == -1)
2656  {
2657  start_id = nid;
2658  }
2659  else
2660  {
2661  end_id = nid;
2662  if (end_id - start_id > 1) // apply optimization
2663  {
2664  // path segment set up
2665  vector<iro::SE2> segment;
2666  for (int tid = start_id; tid <= end_id; tid++)
2667  segment.push_back(m_sync_move_paths[rid][tid]);
2668  // optimize by energy function
2669  bool rtn = Optimize_Path(m_p_de, segment);
2670 cerr << "Optimize_Path success " << rtn << endl;
2671  // save
2672  for (int sid = 0, tid = start_id + 1; tid < end_id; sid++, tid++)
2673  {
2674  m_sync_move_paths[rid][tid].translation().x() = segment[sid].translation().x();
2675  m_sync_move_paths[rid][tid].translation().y() = segment[sid].translation().y();
2676  }
2677  }
2678  start_id = end_id;
2679  end_id = -1;
2680  }
2681  }
2682  }// optimization end
2683  }
2684  // delta_angle
2685  double delta_angle = g_angleDifference;
2686  // angle optimization quick // to achieve cover uncertainty along path
2687  {
2688  // interpolate temp_path
2689  vector<iro::SE2> temp_path;
2690  // node 0 ~ last-1
2691  for (int nid = 0; nid < m_sync_move_paths[rid].size() - 1; nid++)
2692  {
2693  // this node not nbv
2694  if (fabs(m_sync_move_paths[rid][nid].rotation().angle() - empty_angle) < 0.01)
2695  {
2696  // position
2697  Eigen::Vector2d beg(m_sync_move_paths[rid][nid].translation().x(), m_sync_move_paths[rid][nid].translation().y());
2698  Eigen::Vector2d end(m_sync_move_paths[rid][nid + 1].translation().x(), m_sync_move_paths[rid][nid + 1].translation().y());
2699  Eigen::Vector2d move_vector(end - beg);
2700  // angle
2701  Eigen::Vector2d base(0, 1);
2702  Eigen::Vector2d dire(m_sync_move_paths[rid][nid + 1].translation().x() - m_sync_move_paths[rid][nid].translation().x(), m_sync_move_paths[rid][nid + 1].translation().y() - m_sync_move_paths[rid][nid].translation().y());
2703  // move direction
2704  double d_angle = acos(base.dot(dire) / (dire.norm() * base.norm()));
2705  if (dire.x() > 0)
2706  d_angle = -d_angle;
2707  // 2 branches
2708  {
2709  if (nid != 0)
2710  {
2711  // last angle
2712  double lst_ang = m_sync_move_paths[rid][nid - 1].rotation().angle();
2713  {
2714  if (fabs(lst_ang - empty_angle) < 0.01)
2715  {
2716  lst_ang = temp_path.back().rotation().angle();
2717  }
2718  if (lst_ang < 0)
2719  {
2720  lst_ang += 2 * PI;
2721  }
2722  }
2723  // current angle
2724  double crt_ang = d_angle;
2725  {
2726  if (crt_ang < 0)
2727  {
2728  crt_ang += 2 * PI;
2729  }
2730  }
2731  if (crt_ang - lst_ang < 0)
2732  {
2733  // left
2734  {
2735  double l_angle = d_angle + delta_angle;
2736  if (l_angle > PI)
2737  l_angle -= 2 * PI;
2738  Eigen::Vector2d p(beg);
2739  temp_path.push_back(iro::SE2(p.x(), p.y(), l_angle));
2740  }
2741  // mid
2742  {
2743  Eigen::Vector2d p(beg + 0.33*move_vector);
2744  temp_path.push_back(iro::SE2(p.x(), p.y(), d_angle));
2745  }
2746  // right
2747  {
2748  double r_angle = d_angle - delta_angle;
2749  if (r_angle < -PI)
2750  r_angle += 2 * PI;
2751  Eigen::Vector2d p(beg + 0.67*move_vector);
2752  temp_path.push_back(iro::SE2(p.x(), p.y(), r_angle));
2753  }
2754  }
2755  else
2756  {
2757  // right
2758  {
2759  double r_angle = d_angle - delta_angle;
2760  if (r_angle < -PI)
2761  r_angle += 2 * PI;
2762  Eigen::Vector2d p(beg);
2763  temp_path.push_back(iro::SE2(p.x(), p.y(), r_angle));
2764  }
2765  // mid
2766  {
2767  Eigen::Vector2d p(beg + 0.33*move_vector);
2768  temp_path.push_back(iro::SE2(p.x(), p.y(), d_angle));
2769  }
2770  // left
2771  {
2772  double l_angle = d_angle + delta_angle;
2773  if (l_angle > PI)
2774  l_angle -= 2 * PI;
2775  Eigen::Vector2d p(beg + 0.67*move_vector);
2776  temp_path.push_back(iro::SE2(p.x(), p.y(), l_angle));
2777  }
2778  }
2779  }
2780  else
2781  {
2782  // left
2783  {
2784  double l_angle = d_angle + delta_angle;
2785  if (l_angle > PI)
2786  l_angle -= 2 * PI;
2787  Eigen::Vector2d p(beg);
2788  temp_path.push_back(iro::SE2(p.x(), p.y(), l_angle));
2789  }
2790  // mid
2791  {
2792  Eigen::Vector2d p(beg + 0.33*move_vector);
2793  temp_path.push_back(iro::SE2(p.x(), p.y(), d_angle));
2794  }
2795  // right
2796  {
2797  double r_angle = d_angle - delta_angle;
2798  if (r_angle < -PI)
2799  r_angle += 2 * PI;
2800  Eigen::Vector2d p(beg + 0.67*move_vector);
2801  temp_path.push_back(iro::SE2(p.x(), p.y(), r_angle));
2802  }
2803  }
2804  }
2805  } // end of interpolate
2806  // this node not nbv
2807  else
2808  {// interpolate for nbv
2809  // interpolate method 2
2810  {
2811  // push nbv
2812  temp_path.push_back(m_sync_move_paths[rid][nid]);
2813  }
2814  }
2815  }
2816  // node last
2817  if (fabs(m_sync_move_paths[rid].back().rotation().angle() - empty_angle) < 0.01)
2818  m_sync_move_paths[rid].back().rotation().angle() = m_robot_move_views[rid].back().rotation().angle();
2819  temp_path.push_back(m_sync_move_paths[rid].back());
2820  // save temp_path 2 syn_move_path
2821  m_sync_move_paths[rid].clear();
2822  for (int nid = 0; nid < temp_path.size(); nid++)
2823  m_sync_move_paths[rid].push_back(temp_path[nid]);
2824  }
2825 //*/
2826  }
2827  return true;
2828 }
2829 
2830 // camera trajectory interpolation: delta distance/angle constraints.
2831 void Navigation::trajectoryInterpolation(vector<iro::SE2> & path, double dAngle)
2832 {
2833  vector<iro::SE2> result;
2834  for (int i = 0; i < path.size() - 1; i++)
2835  {
2836  int ii = i + 1;
2837  double nTheta = path[ii].rotation().angle();
2838  if (nTheta < 0) nTheta += 2 * PI;
2839  double cTheta = path[i].rotation().angle();
2840  if (cTheta < 0) cTheta += 2 * PI;
2841  double difference = nTheta - cTheta;
2842  if (difference > PI) difference = difference - 2 * PI;
2843  if (difference < -PI) difference = difference + 2 * PI;
2844  // push node i
2845  result.push_back(path[i]);
2846  // if > aAngle, push interpolated node
2847  if (fabs(difference) > dAngle)
2848  {
2849  int num = ceil(fabs(difference) / dAngle) + 1; // #segment
2850  Eigen::Vector2d crt(path[i].translation());
2851  Eigen::Vector2d nxt(path[ii].translation());
2852  Eigen::Vector2d dlt = (nxt - crt) / num;
2853  cerr << "two end point" << endl << path[i].translation().transpose() << " " << path[i].rotation().angle() << endl << path[ii].translation().transpose() << " " << path[ii].rotation().angle() << endl;
2854  // interpolation
2855  for (int id = 1; id < num; id++)
2856  {
2857  // position
2858  Eigen::Vector2d itp_position = crt + dlt*id;
2859  // orientation
2860  double itp_theta = path[i].rotation().angle() + difference / num * id;
2861  if (itp_theta > PI) itp_theta = itp_theta - 2 * PI;
2862  if (itp_theta < -PI) itp_theta = itp_theta + 2 * PI;
2863  result.push_back(iro::SE2(itp_position.x(), itp_position.y(), itp_theta));
2864  cerr << "interpolate " << itp_position.transpose() << " " << itp_theta << endl;
2865  }
2866  }
2867  }
2868  // push the last node
2869  result.push_back(path.back());
2870  // save
2871  path.clear();
2872  for (int i = 0; i < result.size(); i++)
2873  path.push_back(result[i]);
2874  return;
2875 }
2876 
2877 // traj opt, not finish
2878 void Navigation::Trajectory_Optimization()
2879 {
2880 // input: m_robot_move_views
2881 // output: m_sync_move_paths
2882  // setup
2883 /*
2884  cover_map.clear();
2885  cover_map.resize(row);
2886  for (int r = 0; r < cover_map.size(); r++)
2887  {
2888  cover_map[r].resize(col);
2889  for (int c = 0; c < cover_map[r].size(); c++)
2890  {
2891  cover_map[r][c] = 0;
2892  }
2893  }
2894 //*/
2895  //checkTrojectories();
2896  // optimize move paths
2897  vector<double> distances;
2898  // get all geodesic nodes
2899  cerr << "compute geodesic nodes...";
2900  compute_robot_move_nodes(distances);
2901  cerr << "done." << endl;
2902 /*
2903  // test vis. ok.
2904  {
2905  cv::Mat statement = m_p_de->visCellMap();
2906  for (int rid = 0; rid < rbt_num; ++rid)
2907  {
2908  for (int vid = 0; vid < m_robot_move_nodes[rid].size(); ++vid)
2909  {
2910  cv::circle(statement, cv::Point((int)m_robot_move_nodes[rid][vid].translation().x(), -(int)m_robot_move_nodes[rid][vid].translation().y()), 3, CV_RGB(0, 0, 255));
2911  if (vid>0)
2912  {
2913  cv::line(statement,
2914  cv::Point((int)m_robot_move_nodes[rid][vid].translation().x(), -(int)m_robot_move_nodes[rid][vid].translation().y()),
2915  cv::Point((int)m_robot_move_nodes[rid][vid-1].translation().x(), -(int)m_robot_move_nodes[rid][vid-1].translation().y()),
2916  CV_RGB(0, 0, 255));
2917  }
2918  }
2919  }
2920  cv::imshow("plan move nodes", statement);
2921  cv::waitKey(0);
2922  }
2923 //*/
2924  // uniform sample and optimize
2925  cerr << "path optimization...";
2926  compute_sync_move_paths(distances);
2927  cerr << "done." << endl;
2928 /*
2929  // test vis. ok.
2930  {
2931  cv::Mat statement = m_p_de->visCellMap();
2932  for (int rid = 0; rid < rbt_num; ++rid)
2933  {
2934  for (int vid = 0; vid < m_sync_move_paths[rid].size(); ++vid)
2935  {
2936  cv::circle(statement, cv::Point((int)m_sync_move_paths[rid][vid].translation().x(), -(int)m_sync_move_paths[rid][vid].translation().y()), 3, CV_RGB(0, 0, 255));
2937  if (vid>0)
2938  {
2939  cv::line(statement,
2940  cv::Point((int)m_sync_move_paths[rid][vid].translation().x(), -(int)m_sync_move_paths[rid][vid].translation().y()),
2941  cv::Point((int)m_sync_move_paths[rid][vid-1].translation().x(), -(int)m_sync_move_paths[rid][vid-1].translation().y()),
2942  CV_RGB(0, 0, 255));
2943  }
2944  }
2945  }
2946  cv::imshow("plan sync move paths", statement);
2947  cv::waitKey(0);
2948  }
2949 //*/
2950  // trajectory interpolation
2951  if (g_fpInterpolation)
2952  {
2953  // first node orientation
2954  for (int rid = 0; rid < m_sync_move_paths.size(); rid++)
2955  {
2956  m_sync_move_paths[rid][0] = m_p_de->m_pose2d[rid];
2957  }
2958  // interpolation
2959  for (int rid = 0; rid < m_sync_move_paths.size(); rid++)
2960  {
2961  trajectoryInterpolation(m_sync_move_paths[rid]);
2962  }
2963  }
2964  else
2965  {
2966  // first node orientation
2967  for (int rid = 0; rid < m_sync_move_paths.size(); rid++)
2968  {
2969  if (m_sync_move_paths[rid].size() > 1)
2970  {
2971  Eigen::Vector2d xy(m_sync_move_paths[rid][0].translation().x(), m_sync_move_paths[rid][0].translation().y());
2972  Eigen::Vector2d nxy(m_sync_move_paths[rid][1].translation().x(), m_sync_move_paths[rid][1].translation().y());
2973  // compute direction
2974  Eigen::Vector2d direction(nxy - xy);
2975  // same position of two nodes
2976  if (direction.norm() < 0.1) continue;
2977  // compute orientation
2978  direction.normalize();
2979  // convert format
2980  Eigen::Vector2d base(0, 1);
2981  double theta = acos(direction.dot(base));
2982  // convert format
2983  if (direction.x() > 0)
2984  theta = -theta;
2985  double angle = theta;
2986  // save
2987  m_sync_move_paths[rid][0].rotation().angle() = angle;
2988  }
2989  }
2990  }
2991  // vis.
2992  {
2993  cv::Mat statement = m_p_de->visCellMap();
2994  // draw sync path
2995  for (int rid = 0; rid < rbt_num; ++rid)
2996  {
2997  for (int vid = 0; vid < m_sync_move_paths[rid].size(); ++vid)
2998  {
2999  if (vid>0)
3000  {
3001  cv::line(statement,
3002  cv::Point((int)m_sync_move_paths[rid][vid].translation().x(), -(int)m_sync_move_paths[rid][vid].translation().y()),
3003  cv::Point((int)m_sync_move_paths[rid][vid-1].translation().x(), -(int)m_sync_move_paths[rid][vid-1].translation().y()),
3004  CV_RGB(255, 0, 255));
3005  }
3006  cv::circle(statement, cv::Point((int)m_sync_move_paths[rid][vid].translation().x(), -(int)m_sync_move_paths[rid][vid].translation().y()), 2, CV_RGB(0, 0, 255));
3007  }
3008  }
3009 
3010  // test show frontiers
3011  {
3012  // draw frontiers in list
3013  cv::Mat temp = statement.clone();
3014  for (int fid = 0; fid < m_frontierList.size(); fid++)
3015  {
3016  int x = m_frontierList[fid].position.x();
3017  int y = -m_frontierList[fid].position.y();
3018  //cv::circle(temp, cv::Point(x, y), 0, CV_RGB(255, 255, 255));
3019  //cv::circle(temp, cv::Point(x, y), 1, CV_RGB(255, 255, 255));
3020  cv::circle(temp, cv::Point(x, y), 2, CV_RGB(255, 255, 255));
3021  }
3022  // save file
3023  char output_path[200];
3024  sprintf(output_path, "result/frontierList_%d.png", g_plan_iteration);
3025  cv::imwrite(output_path, temp);
3026  }
3027 
3028  // save file
3029  char output_path[200];
3030  sprintf(output_path, "result/plan_%d.png", g_plan_iteration);
3031  cv::imwrite(output_path, statement);
3032  // show
3033  cv::imshow("plan sync move paths", statement);
3034  cv::waitKey(1);
3035  }
3036 //*/
3037  return;
3038 }
3039 
3040 // ask robot to move and scan, not finish
3041 void Navigation::moveRobotsAndScan()
3042 {
3043  // set up
3044  //next_iter_sign = false;
3045  //explore_counter = 0;
3046  //char mode = 's';
3048  //if (mode == 's') // synchronize robots' movement
3049  {
3050  // set up
3051  int min_size = INT_MAX; // max size
3052  bool exist_robot_no_task = false;
3053  vector<bool> robot_no_task_signs(rbt_num);
3054  for (int rid = 0; rid < rbt_num; rid++)
3055  robot_no_task_signs.push_back(false);
3056  // min size of path
3057  for (int rid = 0; rid < rbt_num; rid++)
3058  {
3059  if (m_sync_move_paths[rid].size() == 1)
3060  {
3061  exist_robot_no_task = true;
3062  robot_no_task_signs[rid] = true;
3063  continue;
3064  }
3065  if (min_size > m_sync_move_paths[rid].size())
3066  min_size = m_sync_move_paths[rid].size();
3067  }
3068  if (min_size == INT_MAX)
3069  {
3070  min_size = 1;
3071  }
3072  // move and scan // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
3073  // some robots have been assigned no task
3074  if (exist_robot_no_task)
3075  {
3076  // move and scan
3077  for (int vid = 0; vid < min_size; vid++)
3078  {
3079 /*
3080  // check if need new planning
3081  if (method_num != 4)
3082  {
3083  if (rbt_num == 1)
3084  {
3085  //if (next_iter_sign)
3086  //{
3087  // cerr << " - - - require next motion plan iteration - - - " << endl;
3088  // next_iter_sign = false;
3089  // explore_counter = 0;
3090  // break;
3091  //}
3092  }
3093  else
3094  {
3096  //if (next_iter_sign)
3097  //{
3098  // cerr << " - - - require next motion plan iteration - - - " << endl;
3099  // next_iter_sign = false;
3100  // explore_counter = 0;
3101  // break;
3102  //}
3103  }
3104  }
3105 //*/
3106  // move
3107  vector<vector<double>> pose7s; // get views
3108  for (int rid = 0; rid < rbt_num; rid++)
3109  {
3110  // coordinate transform
3111  vector<double> pose7;
3112  if (robot_no_task_signs[rid])
3113  pose7 = m_p_de->coord_trans_se22gazebo(m_sync_move_paths[rid][0]); // use the first node(only one node in path) pose
3114  else
3115  pose7 = m_p_de->coord_trans_se22gazebo(m_sync_move_paths[rid][vid]); // use current node pose
3116  pose7s.push_back(pose7);
3117  if (robot_no_task_signs[rid])
3118  continue;
3119  // total distance
3120  if (vid != 0)
3121  {
3122  int lid = vid - 1;
3123  // todo: total distance
3124  //total_distance += get_euclidean_distance(Point_2(m_sync_move_paths[rid][vid].translation().x(), m_sync_move_paths[rid][vid].translation().y()), Point_2(m_sync_move_paths[rid][lid].translation().x(), m_sync_move_paths[rid][lid].translation().y()));
3125  }
3126  }
3127  // drive robots to move and scan
3128  cerr<<"socket move to views..."<<endl;
3129  m_p_de->socket_move_to_views(pose7s);
3130  cerr<<"done."<<endl;
3131  // save trajectories
3132  for (int rid = 0; rid < rbt_num; rid++)
3133  {
3134  if (robot_no_task_signs[rid])
3135  {
3136  if (g_rbtTrajectories[rid].empty())
3137  {
3138  g_rbtTrajectories[rid].push_back(Point_2(m_sync_move_paths[rid][0].translation().x(), m_sync_move_paths[rid][0].translation().y()));
3139  //cout << "- record syn_move_path " << m_sync_move_paths[rid][0].translation().x() << ", " << m_sync_move_paths[rid][0].translation().y() << endl;
3140  }
3141  g_camTrajectories[rid].push_back(m_sync_move_paths[rid][0]); // for camera trajecoty. 2018-12-30.
3142  }
3143  else
3144  {
3145  g_rbtTrajectories[rid].push_back(Point_2(m_sync_move_paths[rid][vid].translation().x(), m_sync_move_paths[rid][vid].translation().y()));
3146  //cout << "- record syn_move_path " << m_sync_move_paths[rid][vid].translation().x() << ", " << m_sync_move_paths[rid][vid].translation().y() << endl;
3147  g_camTrajectories[rid].push_back(m_sync_move_paths[rid][vid]); // for camera trajecoty. 2018-12-30.
3148  }
3149  }
3150  // scan and get data
3151  m_p_de->getPoseFromServer();
3152  m_p_de->getRGBDFromServer();
3153  // scan data fusion
3154  // insert scans to tree
3155  //cerr << "fuseScans2MapAndTree() begin" << endl;
3156  m_p_de->fuseScans2MapAndTree();
3157  //cerr << "fuseScans2MapAndTree() done." << endl;
3158  // project octotree to a 2d map
3159  //cerr << "projectOctree2Map() begin" << endl;
3160  m_p_de->projectOctree2Map();
3161  //cerr << "projectOctree2Map() done." << endl;
3162  // vis
3163  vector<iro::SE2> current_views;
3164  for (int rid = 0; rid < rbt_num; rid++)
3165  {
3166  if (m_sync_move_paths[rid].size() <= vid)
3167  current_views.push_back(m_sync_move_paths[rid].back());
3168  else
3169  current_views.push_back(m_sync_move_paths[rid][vid]);
3170  }
3171 
3172 // todo:
3173  visualizeScan(current_views, vid);
3174 /*
3175 // todo:
3176  move_counter++;
3177 //*/
3178  }
3179  }
3180  // each robot have been assigned tasks
3181  else
3182  {
3183  // move and scan
3184  for (int vid = 0; vid < min_size; vid++)
3185  {
3186 /*
3187  // check if need new plan
3188  if (method_num != 4)
3189  {
3190  if (rbt_num == 1)
3191  {
3192  //if (next_iter_sign)
3193  //{
3194  // cerr << " - - - require next motion plan iteration - - - " << endl;
3195  // next_iter_sign = false;
3196  // explore_counter = 0;
3197  // break;
3198  //}
3199  }
3200  else
3201  {
3203  //if (next_iter_sign)
3204  //{
3205  // cerr << " - - - require next motion plan iteration - - - " << endl;
3206  // next_iter_sign = false;
3207  // explore_counter = 0;
3208  // break;
3209  //}
3210  }
3211  }
3212 //*/
3213  // move
3214  vector<vector<double>> pose7s; // get views
3215  for (int rid = 0; rid < rbt_num; rid++)
3216  {
3217  vector<double> pose7 = m_p_de->coord_trans_se22gazebo(m_sync_move_paths[rid][vid]); // coordinate transform
3218  pose7s.push_back(pose7);
3219  // total distance
3220  if (vid != 0)
3221  {
3222  int lid = vid - 1;
3223  // todo: total distance
3224  //total_distance += get_euclidean_distance(Point_2(m_sync_move_paths[rid][vid].translation().x(), m_sync_move_paths[rid][vid].translation().y()), Point_2(m_sync_move_paths[rid][lid].translation().x(), m_sync_move_paths[rid][lid].translation().y()));
3225  }
3226  }
3227  // drive robots to move and scan
3228  cerr<<"socket move to views..."<<endl;
3229  m_p_de->socket_move_to_views(pose7s);
3230  cerr<<"done."<<endl;
3231  // save trajectories
3232  for (int rid = 0; rid < rbt_num; rid++)
3233  {
3234  g_rbtTrajectories[rid].push_back(Point_2(m_sync_move_paths[rid][vid].translation().x(), m_sync_move_paths[rid][vid].translation().y()));
3235  g_camTrajectories[rid].push_back(m_sync_move_paths[rid][vid]); // for camera trajecoty. 2018-12-30.
3236  }
3237  // scan and get data
3238  m_p_de->getPoseFromServer();
3239  m_p_de->getRGBDFromServer();
3240  // scan data fusion
3241  // insert scans to tree
3242  //cerr << "fuseScans2MapAndTree() begin" << endl;
3243  m_p_de->fuseScans2MapAndTree();
3244  //cerr << "fuseScans2MapAndTree() done." << endl;
3245  // project octotree to a 2d map
3246  //cerr << "projectOctree2Map() begin" << endl;
3247  m_p_de->projectOctree2Map();
3248  //cerr << "projectOctree2Map() done." << endl;
3249  // vis
3250  vector<iro::SE2> current_views; // use to vis
3251  for (int rid = 0; rid < rbt_num; rid++)
3252  {
3253  if (m_sync_move_paths[rid].size() <= vid)
3254  current_views.push_back(m_sync_move_paths[rid].back());
3255  else
3256  current_views.push_back(m_sync_move_paths[rid][vid]);
3257  }
3258 
3259 // todo:
3260  visualizeScan(current_views, vid);
3261 /*
3262 // todo:
3263  move_counter++;
3264 //*/
3265  }
3266  }
3267  }
3268 //*/
3269  return;
3270 }
3271 
3272 // draw views
3273 bool draw_robot_views(cv::Mat & input, vector<iro::SE2> views)
3274 {
3275  const double frustumSize = 20; // show in 20 voxels lenth.
3276  // draw frustums
3277  for (int vid = 0; vid < views.size(); vid++)
3278  {
3279  // position
3280  cv::Point p((int)round(views[vid].translation().x()), -(int)round(views[vid].translation().y()));
3281  cv::circle(input, p, 0, CV_RGB(0, 0, 255), 2, CV_AA);
3282  cv::circle(input, p, 1, CV_RGB(0, 0, 255), 2, CV_AA);
3283  cv::circle(input, p, 2, CV_RGB(0, 0, 255), 2, CV_AA);
3284  cv::circle(input, p, 3, CV_RGB(0, 0, 255), 2, CV_AA);
3285  cv::circle(input, p, 4, CV_RGB(0, 0, 255), 2, CV_AA);
3286  cv::circle(input, p, 5, CV_RGB(0, 0, 255), 2, CV_AA);
3287  // orentation
3288  Eigen::Vector2d d(0, frustumSize);
3289  Eigen::Matrix2d m;
3290  double theta = views[vid].rotation().angle();
3291  m << cos(theta), -sin(theta),
3292  sin(theta), cos(theta);
3293  d = m*d;
3294  // view frustum l
3295  m << cos(PI / 6), -sin(PI / 6),
3296  sin(PI / 6), cos(PI / 6);
3297  d = m*d;
3298  cv::Point l((int)round(views[vid].translation().x() + d.x()), -(int)round(views[vid].translation().y() + d.y()));
3299  cv::line(input, p, l, CV_RGB(0, 0, 255), 1, CV_AA);
3300  // view frustum r
3301  m << cos(-PI / 3), -sin(-PI / 3),
3302  sin(-PI / 3), cos(-PI / 3);
3303  d = m*d;
3304  cv::Point r((int)round(views[vid].translation().x() + d.x()), -(int)round(views[vid].translation().y() + d.y()));
3305  cv::line(input, p, r, CV_RGB(0, 0, 255), 1, CV_AA);
3306  // view frustum lr
3307  cv::line(input, l, r, CV_RGB(0, 0, 255), 1, CV_AA);
3308  }
3309  return true;
3310 }
3311 
3312 // visualization, not finish
3313 bool Navigation::visualizeScan(vector<iro::SE2> current_views, int vid) // todo: remove vid.
3314 {
3315  // recon
3316  cv::Mat statement = m_p_de->visCellMap();
3317  // robot position
3318  for (int i = 0; i < current_views.size(); ++i)
3319  {
3320  cv::circle(statement, cv::Point((int)round(current_views[i].translation().x()), (int)round(-current_views[i].translation().y())), 3, CV_RGB(0, 0, 255));
3321  }
3322  draw_robot_views(statement, current_views);
3323  //cv::imshow("robot positions", statement);
3324  //cv::waitKey(1);
3325  char output_path[200];
3326  sprintf(output_path, "result/_progressive_%d_%d.png", g_plan_iteration, vid);
3327  cv::imwrite(output_path, statement);
3328  return true;
3329 }
Navigation::checkViewRayValidness
bool checkViewRayValidness(cv::Point source, cv::Point target)
Definition: navigation.cpp:1211
d
d
uchar
unsigned char uchar
Definition: data_engine.cpp:6
cos
EIGEN_DEVICE_FUNC const CosReturnType cos() const
Definition: ArrayCwiseUnaryOps.h:202
Point_2
K::Point_2 Point_2
Definition: global.h:32
Navigation::compute_sync_move_paths
bool compute_sync_move_paths(std::vector< double > distances)
Definition: navigation.cpp:2631
g_plan_iteration
int g_plan_iteration
Definition: global.cpp:17
Navigation::uniformSampleWithNBVInfo
std::vector< iro::SE2 > uniformSampleWithNBVInfo(const int robot_id, const double step, const double length)
Definition: navigation.cpp:2528
f
f
Optimize_Path
bool Optimize_Path(DataEngine *p_de, vector< iro::SE2 > &path)
Definition: path_optimization.h:282
Navigation::extractTasks
bool extractTasks(Polygon_2 boundary, std::vector< Polygon_2 > holes, std::vector< ScanningTask > &tasks)
Definition: navigation.cpp:1670
g_scene_boundary
std::vector< cv::Point > g_scene_boundary
Definition: global.cpp:19
g_camTrajectories
std::vector< std::vector< iro::SE2 > > g_camTrajectories
Definition: global.cpp:14
Navigation::get_frustum_contuor
vector< cv::Point > get_frustum_contuor(iro::SE2 pose)
Definition: navigation.cpp:1286
Navigation::computeLoationMap
cv::Mat computeLoationMap()
Definition: navigation.cpp:1181
s
XmlRpcServer s
sqrt
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
Definition: ArrayCwiseUnaryOps.h:152
Navigation::simplifyPolygon
bool simplifyPolygon(Polygon_2 &poly, const double colinearThresh, bool addNoise)
Definition: navigation.cpp:327
Navigation::points_equal
bool points_equal(Point_2 p1, Point_2 p2)
Definition: navigation.cpp:2508
Navigation::differenceCGALExactKernel
Pwh_list_2 differenceCGALExactKernel(Polygon_with_holes_2 domain, Polygon_2 hole)
Definition: navigation.cpp:381
iro::SE2::rotation
const Eigen::Rotation2Dd & rotation() const
Definition: se2.h:63
Thread::start
void start()
Definition: threads.cpp:5
Thread::my_id
int my_id
Definition: threads.h:67
DiscreteSolver::getSolution
std::vector< std::vector< int > > getSolution()
Definition: omt_solver.h:120
cdt_obj_path
const std::string cdt_obj_path
Definition: global.h:10
PI
#define PI
Definition: global.h:13
Thread::mytsp
TSP * mytsp
Definition: threads.h:64
rbt_num
int rbt_num
Definition: co_scan.cpp:15
Navigation::Trajectory_Optimization
void Trajectory_Optimization()
Definition: navigation.cpp:2878
Navigation::processCurrentScene
void processCurrentScene()
Definition: navigation.cpp:8
Polygon_with_holes_2
CGAL::Polygon_with_holes_2< K > Polygon_with_holes_2
Definition: global.h:49
Navigation::visualizeScan
bool visualizeScan(std::vector< iro::SE2 > current_views, int vid)
Definition: navigation.cpp:3313
abs
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const AbsReturnType abs() const
Definition: ArrayCwiseUnaryOps.h:43
floor
EIGEN_DEVICE_FUNC const FloorReturnType floor() const
Definition: ArrayCwiseUnaryOps.h:388
g_rbtTrajectories
std::vector< std::vector< Point_2 > > g_rbtTrajectories
Definition: global.cpp:13
TSP::get_size
int get_size()
Definition: tsp.h:169
Navigation::moveRobotsAndScan
void moveRobotsAndScan()
Definition: navigation.cpp:3041
Navigation::load_and_check_holes
std::vector< Polygon_2 > load_and_check_holes(Polygon_2 boundary, std::vector< Polygon_2 > &origin_holes)
Definition: navigation.cpp:491
Navigation::preprocess_frontiers
std::vector< int > preprocess_frontiers(Polygon_2 outer, std::vector< Polygon_2 > iner)
Definition: navigation.cpp:1016
FrontierElement::position
Point_2 position
Definition: navigation.h:145
Navigation::extractQualityTaskViews
std::vector< NextBestView > extractQualityTaskViews(Polygon_2 boundary, std::vector< Polygon_2 > holes)
Definition: navigation.cpp:1579
Navigation::generateMoveDomainCDT
bool generateMoveDomainCDT(Polygon_2 boundary, std::vector< Polygon_2 > holes, std::vector< Polygon_2 > origin_holes, std::vector< ScanningTask > tasks, CDT &cdt)
Definition: navigation.cpp:878
Thread::start_node
int start_node
Definition: threads.h:61
Navigation::generateViewsFrontiers
std::vector< NextBestView > generateViewsFrontiers(bool enableSampling=false, double sampleRate=0.5)
Definition: navigation.cpp:1343
map_cellsize
const float map_cellsize
Definition: data_engine.h:39
Navigation::correct_boundary_holes
void correct_boundary_holes(Polygon_2 &boundary, std::vector< Polygon_2 > &holes)
Definition: navigation.cpp:562
map_rows
const int map_rows
Definition: data_engine.h:40
TSP::perfect_matching
void perfect_matching()
Definition: tsp.cpp:496
g_angleDifference
double g_angleDifference
Definition: global.cpp:8
domainFilePath
const std::string domainFilePath
Definition: global.h:5
segment
EIGEN_DEVICE_FUNC SegmentReturnType segment(Index start, Index n)
This is the const version of segment(Index,Index).
Definition: BlockMethods.h:888
TSP::findMST_old
void findMST_old()
Definition: tsp.cpp:409
g_fpInterpolation
bool g_fpInterpolation
Definition: global.cpp:10
Navigation::scanFinished
void scanFinished()
Definition: navigation.cpp:1803
TSP::make_shorter
void make_shorter()
Definition: tsp.cpp:651
Navigation::path_occlusion_check
bool path_occlusion_check(cv::Mat background, cv::Point beg, cv::Point end)
Definition: navigation.cpp:2243
g_distance_step
double g_distance_step
Definition: global.cpp:6
poly_diff
const std::string poly_diff
Definition: global.h:9
Navigation::trajectoryInterpolation
void trajectoryInterpolation(std::vector< iro::SE2 > &path, double dAngle=PI/12)
Definition: navigation.cpp:2831
TSP::path_vals
double ** path_vals
Definition: tsp.h:119
acos
EIGEN_DEVICE_FUNC const AcosReturnType acos() const
Definition: ArrayCwiseUnaryOps.h:262
TSP
Definition: tsp.h:54
map_cols
const int map_cols
Definition: data_engine.h:41
communicateFile
const std::string communicateFile
Definition: global.h:8
holeFilePath
const std::string holeFilePath
Definition: global.h:6
Thread::join
void join()
Definition: threads.cpp:16
Navigation::se2_equal
bool se2_equal(iro::SE2 p1, iro::SE2 p2)
Definition: navigation.cpp:2518
Pwh_list_2
std::list< Polygon_with_holes_2 > Pwh_list_2
Definition: global.h:50
max
int64_t max(int64_t a, const int b)
Definition: Xin_Wang.cpp:10
compactParam
double compactParam
Definition: global.cpp:5
iro::SE2
Definition: se2.h:49
TSP::create_tour
void create_tour(int)
Definition: tsp.cpp:618
CDT
CGAL::Constrained_Delaunay_triangulation_2< K, TDS, Itag > CDT
Definition: global.h:48
TSP::circuit
vector< int > circuit
Definition: tsp.h:96
empty_angle
const double empty_angle
Definition: se2.h:32
Navigation::OMT_TSP
void OMT_TSP()
Definition: navigation.cpp:1963
Polygon_2
CGAL::Polygon_2< K > Polygon_2
Definition: global.h:33
Navigation::robotMoveDomainProcess
bool robotMoveDomainProcess(Polygon_2 &boundary, std::vector< Polygon_2 > &holes, std::vector< Polygon_2 > &origin_holes)
Definition: navigation.cpp:659
resultFilePath
const std::string resultFilePath
Definition: global.h:7
Navigation::load_and_check_boundary
Polygon_2 load_and_check_boundary()
Definition: navigation.cpp:447
DiscreteSolver::SolveOMT
bool SolveOMT(int mode=0)
Definition: omt_solver.h:1252
Eigen::Rotation2D::angle
EIGEN_DEVICE_FUNC Scalar angle() const
Definition: Rotation2D.h:78
Navigation::TSP_path
std::vector< int > TSP_path(std::vector< Point_2 > points, std::vector< vector< double >> weights)
Definition: navigation.cpp:1812
ceil
EIGEN_DEVICE_FUNC const CeilReturnType ceil() const
Definition: ArrayCwiseUnaryOps.h:402
Navigation::compute_robot_move_nodes
bool compute_robot_move_nodes(std::vector< double > &distances)
Definition: navigation.cpp:2319
Eigen::internal::swap
void swap(scoped_array< T > &a, scoped_array< T > &b)
Definition: Memory.h:602
iro::SE2::translation
const Eigen::Vector2d & translation() const
Definition: se2.h:59
Navigation::load_and_check_frontiers
std::vector< Point_2 > load_and_check_frontiers(Polygon_2 &boundary, std::vector< Polygon_2 > &holes)
Definition: navigation.cpp:1006
sin
EIGEN_DEVICE_FUNC const SinReturnType sin() const
Definition: ArrayCwiseUnaryOps.h:220
Navigation::compute_frontier_tasks
std::vector< NextBestView > compute_frontier_tasks(Polygon_2 boundary, std::vector< Polygon_2 > holes)
Definition: navigation.cpp:1523
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