使用opencv进行机器人自主探索的frontier exploration

参考文献：

1. Brian Yamauchi. A Frontier-BasedApproach for Autonomous Exploration. IEEE International Symposium on Computational Intelligence in Robotics & Automation, 2002 :146-151.

2. Visser, A., Ji, X., van Ittersum, M., González Jaime, L. A., & Stancu, L. A. (2008). Beyond frontier exploration. Lecture Notes in Computer Science, 5001, 113-123.

3. Merlijn van Ittersum, Xingrui-Ji, Luis Gonzalez and Laurentiu Stancu. Natural Boundaries. (2007).

4. Héctor H. González-Baños, Jean-Claude Latombe. Navigation Strategies for Exploring Indoor Environments. The International Journal of Robotics Research, Vol. 21, No. 10–11, October-November 2002, pp. 829-848.

该方法使用的候选点并不保证一定能够到达，需要改进

去除小面积的连通区域

void RemoveSmallRegion(Mat& Src, Mat& Dst, int AreaLimit, int CheckMode, int NeihborMode){for (int i = 0; i < Src.rows; ++i){uchar* iData = Src.ptr<uchar>(i);for (int j = 0; j < Src.cols; ++j){if (iData[j] == 0 || iData[j] == 255) continue;else if (iData[j] < 10){iData[j] = 0;//cout<<'#';  }else if (iData[j] > 10){iData[j] = 255;//cout<<'!';  }}}int RemoveCount = 0;       //记录除去的个数  //记录每个像素点检验状态的标签，0代表未检查，1代表正在检查,2代表检查不合格（需要反转颜色），3代表检查合格或不需检查  Mat Pointlabel = Mat::zeros(Src.size(), CV_8UC1);if (CheckMode == 1){for (int i = 0; i < Src.rows; ++i){uchar* iData = Src.ptr<uchar>(i);uchar* iLabel = Pointlabel.ptr<uchar>(i);for (int j = 0; j < Src.cols; ++j){if (iData[j] < 10){iLabel[j] = 3;}}}}else{for (int i = 0; i < Src.rows; ++i){uchar* iData = Src.ptr<uchar>(i);uchar* iLabel = Pointlabel.ptr<uchar>(i);for (int j = 0; j < Src.cols; ++j){if (iData[j] > 10){iLabel[j] = 3;}}}}vector<Point2i> NeihborPos;  //记录邻域点位置  NeihborPos.push_back(Point2i(-1, 0));NeihborPos.push_back(Point2i(1, 0));NeihborPos.push_back(Point2i(0, -1));NeihborPos.push_back(Point2i(0, 1));if (NeihborMode == 1){NeihborPos.push_back(Point2i(-1, -1));NeihborPos.push_back(Point2i(-1, 1));NeihborPos.push_back(Point2i(1, -1));NeihborPos.push_back(Point2i(1, 1));}else;int NeihborCount = 4 + 4 * NeihborMode;int CurrX = 0, CurrY = 0;//开始检测  for (int i = 0; i < Src.rows; ++i){uchar* iLabel = Pointlabel.ptr<uchar>(i);for (int j = 0; j < Src.cols; ++j){if (iLabel[j] == 0){//********开始该点处的检查**********  vector<Point2i> GrowBuffer;                                      //堆栈，用于存储生长点  GrowBuffer.push_back(Point2i(j, i));Pointlabel.at<uchar>(i, j) = 1;int CheckResult = 0;                                               //用于判断结果（是否超出大小），0为未超出，1为超出  for (int z = 0; z<GrowBuffer.size(); z++){for (int q = 0; q<NeihborCount; q++)                                      //检查四个邻域点  {CurrX = GrowBuffer.at(z).x + NeihborPos.at(q).x;CurrY = GrowBuffer.at(z).y + NeihborPos.at(q).y;if (CurrX >= 0 && CurrX<Src.cols&&CurrY >= 0 && CurrY<Src.rows)  //防止越界  {if (Pointlabel.at<uchar>(CurrY, CurrX) == 0){GrowBuffer.push_back(Point2i(CurrX, CurrY));  //邻域点加入buffer  Pointlabel.at<uchar>(CurrY, CurrX) = 1;           //更新邻域点的检查标签，避免重复检查  }}}}if (GrowBuffer.size()>AreaLimit) CheckResult = 2;                 //判断结果（是否超出限定的大小），1为未超出，2为超出  else { CheckResult = 1;   RemoveCount++; }for (int z = 0; z<GrowBuffer.size(); z++)                         //更新Label记录  {CurrX = GrowBuffer.at(z).x;CurrY = GrowBuffer.at(z).y;Pointlabel.at<uchar>(CurrY, CurrX) += CheckResult;}//********结束该点处的检查**********  }}}CheckMode = 255 * (1 - CheckMode);//开始反转面积过小的区域  for (int i = 0; i < Src.rows; ++i){uchar* iData = Src.ptr<uchar>(i);uchar* iDstData = Dst.ptr<uchar>(i);uchar* iLabel = Pointlabel.ptr<uchar>(i);for (int j = 0; j < Src.cols; ++j){if (iLabel[j] == 2){iDstData[j] = CheckMode;}else if (iLabel[j] == 3){iDstData[j] = iData[j];}}}}

基于栅格地图的探索

分辨率：5cm * 5cm per grid

// TODO to calculate the targetPoint according to gridmap

// 自主探索if (IsGoToTarget && !IsManualTarget && !IsSetTarget && autodrive){while (IsGetNewTarget || m_targetPoint.distance2DTo(curPose3D.x(), curPose3D.y()) < 0.5 || IsFirstTarget){global_path.clear();while (!IsGridMapSaved){ArUtil::sleep(5);}IsMapLoading = true;Mat src = imread(tmpGridMapFile, 0);xmin = x_min, xmax = x_max, ymin = y_min, ymax = y_max;IsMapLoading = false;int width = src.cols, height = src.rows;//*************************// walls extraction//*************************Mat black;threshold(src, black, 30, 255, CV_THRESH_BINARY_INV);Mat dilate_black;dilate(black, dilate_black, element7);Mat erode_black;erode(dilate_black, erode_black, element6);Mat wall = Mat::zeros(erode_black.size(), CV_8UC1);RemoveSmallRegion(erode_black, wall, 30, 1, 1);//namedWindow("wall", 1);//imshow("wall", wall);//*************************// clear areas extraction//*************************Mat white;threshold(src, white, 250, 255, CV_THRESH_BINARY);Mat dilate_white;dilate(white, dilate_white, element7);Mat erode_white;erode(dilate_white, erode_white, element7);Mat clear_area = Mat::zeros(erode_white.size(), CV_8UC1);RemoveSmallRegion(erode_white, clear_area, 30, 1, 1);//namedWindow("cleararea", 2);//imshow("cleararea", clear_area);//****************************// shadow regions extraction//****************************Mat gray128_255;threshold(src, gray128_255, 127, 255, CV_THRESH_TOZERO);Mat gray128_250;threshold(gray128_255, gray128_250, 250, 255, CV_THRESH_TOZERO_INV);Mat gray;threshold(gray128_250, gray, 127, 255, CV_THRESH_BINARY);Mat true_gray = gray - dilate_black - clear_area;Mat dilate_gray;dilate(true_gray, dilate_gray, element7);Mat erode_gray;erode(dilate_gray, erode_gray, element6);Mat shadow = Mat::zeros(erode_gray.size(), CV_8UC1);RemoveSmallRegion(erode_gray, shadow, 200, 1, 1);//namedWindow("shadow", 3);//imshow("shadow", shadow);//*************************// extract frontier point//*************************vector<vector<Point> > contours;findContours(shadow, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);int size = contours.size();if (size == 0){IsGoToTarget = false;break;}float radius;Point2f center;vector<Point> candidate_points;for (int i = 0; i < size; i++){minEnclosingCircle(Mat(contours[i]), center, radius);candidate_points.push_back(Point(center));}double best_score = 0, tmp_score = 0;Point best_pt, tmp_pt;vector<Point> best_contour;Point robot((curPose3D.x() - xmin) * 20, (-curPose3D.y() - ymin) * 20);for (int i = 0; i < size; i++){double area = contourArea(contours[i]);tmp_pt = candidate_points[i];double dist = distanceBetweenPoints(tmp_pt.x, tmp_pt.y, robot.x, robot.y);tmp_score = area * exp(-0.0005 * dist);QString qstr = QString("%1, %2, %3").arg(i).arg(area).arg(tmp_score);sendMessage(qstr);if (tmp_score > best_score){best_score = tmp_score;best_pt = tmp_pt;best_contour = contours[i];}}m_targetPoint = CPoint2D(best_pt.x * 0.05 + xmin, best_pt.y * -0.05 + ymin);//*************************// A-Star to get path//*************************vector<vector<int>> maze;width = dilate_black.cols, height = dilate_black.rows;for (int i = 0; i < width; i += 2){vector<int> tmp;for (int j = 0; j < height; j += 2){if (dilate_black.at<uchar>(j, i) >= 250)tmp.push_back(1);elsetmp.push_back(0);}maze.push_back(vector<int>(tmp));}Astar astar;astar.InitAstar(maze);AStarPoint start(robot.x / 2, robot.y / 2);AStarPoint end(best_pt.x / 2, best_pt.y / 2);std::list<AStarPoint *> path = astar.GetPath(start, end, false);int path_size = path.size();if (path_size == 0){sendMessage("cannot reach target!");IsGoToTarget = false;break;}else{sendMessage("reach goal!");int num = 1;for (std::list<AStarPoint *>::iterator it = path.begin(); it != path.end(); it++){AStarPoint * pt = *it;if (num % 5 == 0){CPoint2D tmppt = CPoint2D(pt->x * 0.1 + xmin, pt->y * -0.1 - ymin);global_path.push_back(CPoint2D(tmppt));num = 1;}else{num++;}circle(wall, Point(pt->x * 2, pt->y * 2), 1, Scalar(128));}}sendMessage(QString("target: x: %1, y:%2").arg(m_targetPoint.x()).arg(m_targetPoint.y()));namedWindow("wall", 1);imshow("wall", wall);IsFirstTarget = false;IsGetNewTarget = false;}}