Beispiel #1
0
void MatchSet :: add_match( const PointT point_c, const PointT point_r, float strength, int ref_idx ){
    Match match;
    match.point_c = PointT(point_c);
    match.point_r = PointT(point_r);
    match.strength = strength;
    match.ref_idx = ref_idx;
    _matches.push_back(match);
}
Beispiel #2
0
void MatchSet :: get_matches( vector<Match> &matches ) const{
    for (unsigned int i = 0; i < _matches.size(); i++ ){
        Match match;
        match.point_c = PointT(_matches[i].point_c);
        match.point_r = PointT(_matches[i].point_r);
        match.strength = _matches[i].strength;
        match.ref_idx = _matches[i].ref_idx;
        matches.push_back(match);
    }
}
void RedlineExtractorNode::processImage(void)
{
    cv::Vec3b color;
    double excessRedValue;

    //  Reset points of interest point cloud and image
    this->poiCloud.clear();
    this->poiImage = cv::Mat::zeros(this->input.cvImage->image.rows, this->input.cvImage->image.cols, CV_8UC3);

    //  Iterate through pixels of image (TODO: Faster method exists)
    for (int i = 0; i < this->input.cvImage->image.rows; i++)
    {
        for (int j = 0; j < this->input.cvImage->image.cols; j++)
        {

            color = this->input.cvImage->image.at<cv::Vec3b>(i,j);                          //  Get color of current pixel
            excessRedValue =    this->params.redWeight      * color.val[2] -
                                this->params.greenWeight    * color.val[1] -
                                this->params.blueWeight     * color.val[0];                 //  Calculate excess red value

            if (excessRedValue > this->params.threshold)
            {
                this->poiImage.at<cv::Vec3b>(i,j) = cv::Vec3b(255, 255, 255);               //  Assign white to the pixel of interest

                //  TODO: Homography
                this->poiCloud.push_back(PointT((double)j,(double)i,0.0f));                 //  Add interest point to the point cloud
            }
        }
    }
}
Extractors::RowExtractor::RowExtractor()
{
	//	Setup default parameters
	this->systemParameters.timeLimit = 1000;

	//	Pre-Processor
	this->systemParameters.preProcessor.active = false;
	this->systemParameters.preProcessor.minimumClearZoneDistance = 0.0;
	this->systemParameters.preProcessor.maximumClearZoneDistance = 0.0;

	//	Ransac processor
	this->systemParameters.ransacProcessor.numberOfRansacTries = 0;
	this->systemParameters.ransacProcessor.numberOfPointsToAcceptLine = 0;
	this->systemParameters.ransacProcessor.distanceFromLineThreshold = 0.0;
	this->systemParameters.ransacProcessor.minimumRowLength = 0.0;

	//	Setup input (empty)
	this->input.time_sec = 0;
	this->input.time_nsec = 0;
	this->input.pointCloud = pcl::PointCloud<PointT>();

	//	Setup output (empty)
	this->output.rowFound = false;
	this->output.length = 0.0;
	this->output.orientation = 0.0;
	this->output.center = PointT();
}
 static PointT apply(PointT const & p1,
                       PointT const & p2)
 {
   return PointT(p1[1]*p2[2]-p1[2]*p2[1],
                   p1[2]*p2[0]-p1[0]*p2[2],
                   p1[0]*p2[1]-p1[1]*p2[0]);
 }
void Extractors::RowExtractor::rowProcessor (void)
{
	//	Variables
	const bool debug = false;
	const double lowerBoundPercentage = 0.1;
	double totalSizeOfCloud = (double)this->preProcessedData.size();
	double percentageLeft = 1.0;
	Row ransacOutput;

	//	Clear output
	this->output.rows.clear();
	this->output.rowFound = false;

	//	Find row(s) in point cloud with ransac
	while (percentageLeft > lowerBoundPercentage)
	{
		ransacOutput = ransacProcessor();

		this->output.rows.push_back(ransacOutput);

		percentageLeft = (double)this->preProcessedData.size() / totalSizeOfCloud;
	}

	//	Process row(s) and finalize output
	int tempCount = 0;
	double meanOrientation;

	for (int i = 0; i < this->output.rows.size(); i++)
	{
		this->output.rowFound |= this->output.rows[i].rowFound;

		if (this->output.rows[i].rowFound)
		{
			meanOrientation += this->output.orientation;
			tempCount++;
		}
	}

	if (tempCount)
	{
		this->output.rowFound = true;
		meanOrientation /= (double)tempCount;
		this->output.orientation = meanOrientation;
		this->output.length = 0.0;
		this->output.center.x = 0;
		this->output.center.y = 0;
	}
	else
	{
		this->output.rowFound = false;
		this->output.orientation = giveMeNan();
		this->output.length = giveMeNan();
		this->output.center = PointT();
	}
}
Extractors::RowExtractor::RowExtractor(Parameters par)
{
	//	Setup default parameters
	this->systemParameters = par;

	//	Setup input (empty)
	this->input.time_sec = 0;
	this->input.time_nsec = 0;
	this->input.pointCloud = pcl::PointCloud<PointT>();

	//	Setup output (empty)
	this->output.rowFound = false;
	this->output.length = 0.0;
	this->output.orientation = 0.0;
	this->output.center = PointT();
}
Beispiel #8
0
 PointT operator-(){
     return PointT(-x, -y);
 }
Beispiel #9
0
 PointT operator-(PointT pt){
     return PointT(x - pt.x, y - pt.y);
 }
Beispiel #10
0
 PointT operator+(PointT pt){
     return PointT(x + pt.x, y + pt.y);
 }
Beispiel #11
0
 PointT operator/(T coef){
     return PointT(x / coef, y / coef);
 }
Beispiel #12
0
 PointT operator*(T coef){
     return PointT(x * coef, y * coef);
 }
 static PointT apply(PointT,
                       PointT)
 {
   return PointT(0,0);
 }
Beispiel #14
0
 PointT cw(){
     return PointT(y, -x);
 }
Extractors::Row Extractors::RowExtractor::ransacProcessor (void)
{
	const bool debug = false;

	//	Setup empty row
	Row row;
	row.rowFound = false;
	row.pointInRow = PointT();
	row.lengthToRow = 0.0;
	row.orientation = 0.0;
	row.varianceOfRes = 0.0;

	if (debug) printf(" Ransac processing:\n=========================================\n");

	//	If points available in cloud, process them
	if (this->preProcessedData.size())
	{
		if (debug) printf(" Processing %d points in cloud\n", (int)this->preProcessedData.size());

		//	Seed randomizer
		srand(time(0));

		//	Variables
		int ransacTryCounter = 0;				//	Counter for ransac tries
		int randomPoints[2];					//	Random index from cloud
		int randomTryCounter;					//	Counter of the number of random points selected
		const int maxRandomCount = 10;			//	Maximum number of random tries (NB not ransac tries)
		PointT lookingAt[2];					//	Currently looking at points
		PointT pointsOfInterest[2];				//	Points selected for further analysis
		PointT lineDirectionVector;				//	Direction vector for line (vector between points of interest)
		double lineLength;						//	Length of direction vector
		double lineRotation;					//	Line rotation
		bool lineFound;							//	Did ransac select points far enough away from each other
		bool rowFound = false;					//	Row found?

		PointT currentPoint;					//	Current point to test
		PointT currentVector;					//	Current vector (from lookingAt[0] to currentPoint)
		PointT orthogonalVector;				//	Vector perpendicular to direction vector
		PointT xProjection;						//	Projection of currentVector onto lineDirectionVector
		PointT yProjection;						//	Projection of currentVector onto orthogonalVector
		double xDotProduct;						//	Dot product between currentVector and lineDirectionVector
		double yDotProduct;						//	Dot product between currentVector and orthogonalVector
		double currentVecLen;					//	Length of current vector
		double orthoVecLen;						//	Length of orthogonal vector
		double xProjLen;						//	Length of projection of currentVector onto lineDirectionVector
		double yProjLen;						//	Length of projection of currentVector onto orthogonalVector

		int ransacInlierCounter;				//	Count inlier points
		int ransacBestCount = 0;				//	Best inlier ransac count

		std::list<double> xProjTestDist;		//	Tentative placeholder of length of vectors projected onto line
		std::list<double> xProjDistribution;	//	Store projection length
		std::list<double> weights;				//	Store weights from xProjDistribution
		std::list<double> residuals;			//	Store residuals from linear regression on CDF of points along the line

		std::vector<int> inlierIndecies;		//	Storing indecies of inlier points
		inlierIndecies = std::vector<int>();

		double inliersPercentage;				//	Percentage of inliers compared to the total number of points in the pre-processed cloud

		//	Find possible row
		while (ransacTryCounter < this->systemParameters.ransacProcessor.numberOfRansacTries)
		{
			ransacTryCounter++;

			inlierIndecies.clear();
			ransacBestCount = 0;
			randomTryCounter = 0;
			lineFound = false;

			do
			{
				randomPoints[0] = rand() % this->preProcessedData.size();
				randomPoints[1] = rand() % this->preProcessedData.size();

				lookingAt[0] = this->preProcessedData.at(randomPoints[0]);
				lookingAt[1] = this->preProcessedData.at(randomPoints[1]);

				//	Calculate direction from two randomly selected points
				lineDirectionVector.x = lookingAt[1].x - lookingAt[0].x;
				lineDirectionVector.y = lookingAt[1].y - lookingAt[0].y;

				lineLength = sqrt(pow(lineDirectionVector.x, 2) + pow(lineDirectionVector.y, 2));

				randomTryCounter++;

				if (lineLength >= this->systemParameters.ransacProcessor.minimumRowLength) lineFound = true;
			} while (!lineFound && randomTryCounter < maxRandomCount);

			//	If proper line is found continue further analysis
			if (lineFound)
			{
				ransacInlierCounter = 0;

				xProjTestDist.clear();
				weights.clear();

				orthogonalVector.x = - lineDirectionVector.y;
				orthogonalVector.y = lineDirectionVector.x;

				//	Traverse all points in cloud
				for (int i = 0; i < this->preProcessedData.size(); i++)
				{
					//	Get current point
					currentPoint = this->preProcessedData[i];

					//	Calculate "TLS" distance
					currentVector.x = currentPoint.x - lookingAt[0].x;
					currentVector.y = currentPoint.y - lookingAt[0].y;

					//	Make projection of currentVector onto directionVector and orthogonalVector
					xDotProduct = currentVector.x * lineDirectionVector.x + currentVector.y * lineDirectionVector.y;
					yDotProduct = currentVector.x * orthogonalVector.x + currentVector.y * orthogonalVector.y;

					currentVecLen = sqrt(pow(currentVector.x, 2) + pow(currentVector.y, 2));
					orthoVecLen = sqrt(pow(orthogonalVector.x, 2) + pow(orthogonalVector.y, 2));

					xProjection.x = xDotProduct / pow(currentVecLen, 2) * currentVector.x;
					xProjection.y = xDotProduct / pow(currentVecLen, 2) * currentVector.y;

					yProjection.x = yDotProduct / pow(orthoVecLen, 2) * orthogonalVector.x;
					yProjection.y = yDotProduct / pow(orthoVecLen, 2) * orthogonalVector.y;

					xProjLen = sqrt(pow(xProjection.x, 2) + pow(xProjection.y, 2));
					yProjLen = sqrt(pow(yProjection.x, 2) + pow(yProjection.y, 2));

					//	If it is an inlier
					if (yProjLen < this->systemParameters.ransacProcessor.distanceFromLineThreshold)
					{
						ransacInlierCounter++;

						//	Add index to inlier index
						inlierIndecies.push_back(i);

						//	If projection onto direction vector is in bound add length to distribution
						if (xDotProduct > 0 && xProjLen < lineLength)
						{
							xProjTestDist.push_back(xProjLen);
						}
					}
				}

				if (ransacInlierCounter > ransacBestCount)
				{
					ransacBestCount = ransacInlierCounter;
					xProjDistribution = xProjTestDist;
				}
			}
		}

		//	If any "best" line was found, analyse quality
		if (xProjDistribution.size())
		{
			//	Calculate the percentage of inliers to line compared with the overall number of points
			inliersPercentage = (double)xProjDistribution.size() / (double)this->preProcessedData.size();

			xProjDistribution.sort();

			if (debug)
			{
				printf(" Number of inliers: %d\n", (int)inlierIndecies.size());
				printf(" Before reduction of point cloud: %d\n", (int)this->preProcessedData.size());
			}

			//	Remove inliers from pre-processed cloud
			for (int index = inlierIndecies.size() - 1; index >= 0; index--)
			{
				this->preProcessedData.erase(preProcessedData.begin() + inlierIndecies[index]);
			}

			if (debug) printf(" After reduction of point cloud: %d\n", (int)this->preProcessedData.size());

			double numberOfPoints = (double)xProjDistribution.size();
			double weight = 1. / numberOfPoints;
			double cumulativeWeight = 0.0;

			for (std::list<double>::iterator it = xProjDistribution.begin(); it != xProjDistribution.end(); ++it)
			{
				cumulativeWeight += weight;
				weights.push_back(cumulativeWeight);
			}

			// Check distribution
			double sumX = 0, sumY = 0, sumXY = 0, sumSquareX = 0;
			double meanX, meanY, meanXY, meanSquareX;
			double alpha, beta;

			for (std::list<double>::iterator itx = xProjDistribution.begin(), ity = weights.begin(); itx != xProjDistribution.end(); ++itx, ++ity)
			{
				sumX += *itx;
				sumY += *ity;
				sumXY += (*itx) * (*ity);
				sumSquareX += pow(*itx, 2);
			}

			meanX = sumX / numberOfPoints;
			meanY = sumY / numberOfPoints;
			meanXY = sumXY / numberOfPoints;
			meanSquareX = sumSquareX / numberOfPoints;

			//	Calculate linear regression parameters
			alpha = (meanXY - meanX * meanY) / (meanSquareX - pow(meanX, 2));
			beta = meanY - alpha * meanX;

			//	Calculate variance of residuals
			double residual, sumRes = 0, sumResSquared = 0, variance;

			for (std::list<double>::iterator itx = xProjDistribution.begin(), ity = weights.begin(); ity != weights.end(); ++itx, ++ity)
			{
				residual = *ity - (alpha * (*itx) + beta);

				sumRes += residual;
				sumResSquared += pow(residual, 2);
			}

			variance = (sumResSquared - pow(sumRes, 2)) / numberOfPoints;
			lineRotation = std::atan2(lineDirectionVector.y, lineDirectionVector.x);

			//	Setup output
			row.rowFound = true;
			row.pointInRow = lookingAt[0];
			row.lengthToRow = lineLength;
			row.orientation = lineRotation;
			row.varianceOfRes = variance;

			if (debug)
			{
				printf(" Point in line: (%f, %f)\n", lookingAt[0].x, lookingAt[0].y);
				printf(" Line length: %f\n", lineLength);
				printf(" Line rotation: %f\n", lineRotation);
				printf(" Variance of residuals: %f\n", variance);
			}
		}
		else
		{
			if (debug) printf(" No fit was made from point cloud!\n");
		}
	}
	else
	{
		if (debug) printf(" No points in point cloud!\n");
	}

	if (debug) printf("=========================================\n Ransac processing ended\n\n\n");

	return row;
}
Beispiel #16
0
std::vector<std::map<std::pair<int, int>, std::string> > Map::analyzeFlowMap(){
  bool found = false;
  //  std::vector<std::map<std::pair<int, int>, std::string> > solution = analyzeDotPair(DotsMap.begin(), initMapInfo, found);
  std::vector<PathNodeT*>* pathList = new std::vector<PathNodeT*>;
  std::map<std::string, DotPairT*>::iterator map_it = DotsMap.begin();

  PointT start = map_it->second->loc1;
  PointT end = map_it->second->loc2;
  
  std::queue<PathNodeT*> BFS;
  std::map<std::pair<int, int>, std::string> tmpMap;
  std::map<std::pair<int, int>, std::string> curMapInfo = initMapInfo;
  PathNodeT* curPathNode = new PathNodeT;
  PointT curPoint;
  PointT nextPoint;
  
  curPathNode->prev = NULL;
  curPathNode->cur = start;
  curPathNode->pathMap = curMapInfo;
  
  BFS.push(curPathNode);
  int count1=0;
  while (!BFS.empty()){
    curPathNode = BFS.front();
    curPoint = curPathNode->cur;
    tmpMap = curPathNode->pathMap;
    BFS.pop();
    /*
    if (tmpMap.size() > size * size * factor){
      delete curPathNode;
      break;
    }
    */
    /*
    if (curPoint.x == end.x && curPoint.y == end.y){
      pathList->push_back(curPathNode);
      continue;
    }
    */
    nextPoint =PointT(curPoint.x - 1, curPoint.y);
    if (nextPoint.x == end.x && nextPoint.y == end.y){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
      pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
      std::cout<<++count1<<map_it->first<<std::endl;
      continue;
    }
    if (!isCollide(curPathNode, nextPoint, curMapInfo)){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
      BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
    }
    
    
    nextPoint =PointT(curPoint.x + 1, curPoint.y);
    if (nextPoint.x == end.x && nextPoint.y == end.y){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
      pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
      std::cout<<++count1<<map_it->first<<std::endl;

      continue;
    }
    if (!isCollide(curPathNode, nextPoint, curMapInfo)){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
      BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
    }
    
    nextPoint =PointT(curPoint.x, curPoint.y - 1);
    if (nextPoint.x == end.x && nextPoint.y == end.y){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
      pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
      std::cout<<++count1<<map_it->first<<std::endl;

      continue;
    }
    if (!isCollide(curPathNode, nextPoint, curMapInfo)){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
      BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
    }
    
    nextPoint =PointT(curPoint.x, curPoint.y + 1);
    if (nextPoint.x == end.x && nextPoint.y == end.y){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
      pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
      std::cout<<++count1<<map_it->first<<std::endl;

      continue;
    }
    if (!isCollide(curPathNode, nextPoint, curMapInfo)){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
      BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
    }
    delete curPathNode;
  }
  // Add one path to map and analyze next dot pair
  std::vector<std::map<std::pair<int, int>, std::string> > solutions;
    
  
  //  for (std::vector<PathNodeT*>::iterator it = pathList->begin(); it != pathList->end(); ++it) {
  omp_lock_t lock;
  omp_init_lock(&lock);
  int count = 0;
  #pragma omp parallel for schedule(dynamic)
  for(int i = 0; i < pathList->size(); ++i){
    
    //nextMapInfo = (*it)->pathMap;
    if (!found){
    std::map<std::pair<int, int>, std::string> nextMapInfo;
    std::vector<std::map<std::pair<int, int>, std::string> > tmpSolutions;
    PathNodeT* tmp;

    nextMapInfo = pathList->at(i)->pathMap;
    /*
    tmp = *it;
    while(tmp){
      nextMapInfo.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(tmp->cur.x, tmp->cur.y), map_it->first));
      //nextColorMap.push_back(DotT(map_it->first, tmp->cur));
      tmp = tmp->prev;
    }*/
    std::map<std::string, DotPairT*>::iterator tmpIt = map_it;
    ++tmpIt;
    if (tmpIt == DotsMap.end()){
      omp_set_lock(&lock);
      solutions.push_back(nextMapInfo);
      omp_unset_lock(&lock);
      /* Modified */
      found = true;
      //break;
      //return solutions;
      /* Modified */
    }
    else{
      tmpSolutions = analyzeDotPair(tmpIt, nextMapInfo, found);// Modified
      omp_set_lock(&lock);
      solutions.insert(solutions.end(), tmpSolutions.begin(), tmpSolutions.end());
      omp_unset_lock(&lock);
      //if(!found)
        //std::cout<<++count<<"\t"<<pathList->size()<<std::endl;
      
      /* Modified */
      //if (found){
      //	break;
      //}
        
    }
    }
  }
  
  return solutions;
}
Beispiel #17
0
void Map::generatePathsForPair(std::string color){
  std::vector<PathNodeT*>* pathList = new std::vector<PathNodeT*>;
  
  PointT start = DotsMap[color]->loc1;
  PointT end = DotsMap[color]->loc2;
  
  std::queue<PathNodeT*> BFS;
  std::map<std::pair<int, int>, std::string> tmpMap;
  PathNodeT* curPathNode = new PathNodeT;
  PointT curPoint;
  PointT nextPoint;
  int count = 0;
  curPathNode->prev = NULL;
  curPathNode->cur = start;
  curPathNode->pathMap = initMapInfo;
  
  BFS.push(curPathNode);
  
  while (!BFS.empty()){
    curPathNode = BFS.front();
    curPoint = curPathNode->cur;
    tmpMap = curPathNode->pathMap;
    BFS.pop();
    if (tmpMap.size()> size*size*factor){
      delete curPathNode;
      break;
    }
    /*
     if (curPoint.x == end.x && curPoint.y == end.y){
     pathList->push_back(curPathNode);
     continue;
     }
     */
    nextPoint =PointT(curPoint.x - 1, curPoint.y);
    if (nextPoint.x == end.x && nextPoint.y == end.y){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), color));
      pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
      continue;
    }
    if (!isCollide(curPathNode, nextPoint, tmpMap)){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), color));
      BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
    }
    
    
    nextPoint =PointT(curPoint.x + 1, curPoint.y);
    if (nextPoint.x == end.x && nextPoint.y == end.y){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), color));
      pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
      continue;
    }
    if (!isCollide(curPathNode, nextPoint, tmpMap)){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), color));
      BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
    }
    
    nextPoint =PointT(curPoint.x, curPoint.y - 1);
    if (nextPoint.x == end.x && nextPoint.y == end.y){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), color));
      pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
      continue;
    }
    if (!isCollide(curPathNode, nextPoint, tmpMap)){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), color));
      BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
    }
    
    nextPoint =PointT(curPoint.x, curPoint.y + 1);
    if (nextPoint.x == end.x && nextPoint.y == end.y){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), color));
      pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
      continue;
    }
    if (!isCollide(curPathNode, nextPoint, tmpMap)){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), color));
      BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
    }
    delete curPathNode;
  }
  
  std::vector<MapInfo> tmp;
  
  for (std::vector<PathNodeT*>::iterator it = pathList->begin();
       it != pathList->end();
       ++it)
    tmp.push_back((*it)->pathMap);
  
  possiblePaths[color] = pathList;
  
  return;
}
Beispiel #18
0
std::vector<std::map<std::pair<int, int>, std::string> > Map::analyzeDotPair(std::map<std::string, DotPairT*>::iterator map_it, std::map<std::pair<int, int>, std::string> curMapInfo, bool &found){
  if (found){
    std::vector<std::map<std::pair<int, int>, std::string> > something;
    return something;
  }

  std::vector<PathNodeT*>* pathList = new std::vector<PathNodeT*>;
  
  PointT start = map_it->second->loc1;
  PointT end = map_it->second->loc2;
  
  std::queue<PathNodeT*> BFS;
  std::map<std::pair<int, int>, std::string> tmpMap;
  PathNodeT* curPathNode = new PathNodeT;
  PointT curPoint;
  PointT nextPoint;
  
  curPathNode->prev = NULL;
  curPathNode->cur = start;
  curPathNode->pathMap = curMapInfo;
  
  BFS.push(curPathNode);
  
  while (!BFS.empty()){
    curPathNode = BFS.front();
    curPoint = curPathNode->cur;
    tmpMap = curPathNode->pathMap;
    BFS.pop();
    /*
    if ((tmpMap.size() - curMapInfo.size()) >= (size * size - curMapInfo.size()) * factor){
      delete curPathNode;
      break;
    }
     */
    /*
    if (curPoint.x == end.x && curPoint.y == end.y){
      pathList->push_back(curPathNode);
      continue;
    }
    */
    nextPoint =PointT(curPoint.x - 1, curPoint.y);
    if (nextPoint.x == end.x && nextPoint.y == end.y){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
      pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
      continue;
    }
    if (!isCollide(curPathNode, nextPoint, curMapInfo)){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
      BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
    }
    
    
    nextPoint =PointT(curPoint.x + 1, curPoint.y);
    if (nextPoint.x == end.x && nextPoint.y == end.y){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
      pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
      continue;
    }
    if (!isCollide(curPathNode, nextPoint, curMapInfo)){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
      BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
    }
    
    nextPoint =PointT(curPoint.x, curPoint.y - 1);
    if (nextPoint.x == end.x && nextPoint.y == end.y){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
      pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
      continue;
    }
    if (!isCollide(curPathNode, nextPoint, curMapInfo)){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
      BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
    }
    
    nextPoint =PointT(curPoint.x, curPoint.y + 1);
    if (nextPoint.x == end.x && nextPoint.y == end.y){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
      pathList->push_back(new PathNodeT(curPathNode, nextPoint, tmpMap));
      continue;
    }
    if (!isCollide(curPathNode, nextPoint, curMapInfo)){
      tmpMap = curPathNode->pathMap;
      tmpMap.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(nextPoint.x, nextPoint.y), map_it->first));
      BFS.push(new PathNodeT(curPathNode, nextPoint, tmpMap));
    }
    delete curPathNode;
  }
  // Add one path to map and analyze next dot pair
  std::map<std::pair<int, int>, std::string> nextMapInfo;
  
  std::vector<std::map<std::pair<int, int>, std::string> > solutions;
  std::vector<std::map<std::pair<int, int>, std::string> > tmpSolutions;
  PathNodeT* tmp;
  for (std::vector<PathNodeT*>::iterator it = pathList->begin(); it != pathList->end(); ++it) {
    nextMapInfo = (*it)->pathMap;
    /*
    tmp = *it;
    while(tmp){
      nextMapInfo.insert(std::pair<std::pair<int, int>, std::string>(std::pair<int, int>(tmp->cur.x, tmp->cur.y), map_it->first));
      //nextColorMap.push_back(DotT(map_it->first, tmp->cur));
      tmp = tmp->prev;
    }*/
    std::map<std::string, DotPairT*>::iterator tmpIt = map_it;
    ++tmpIt;
    if (tmpIt == DotsMap.end()){
      solutions.push_back(nextMapInfo);
      /* Modified */
      found = true;
      return solutions;
      /* Modified */
    }
    else{
      tmpSolutions = analyzeDotPair(tmpIt, nextMapInfo, found);// Modified
      /* Modified */
      if (found){
        return tmpSolutions;
      }
        solutions.insert(solutions.end(), tmpSolutions.begin(), tmpSolutions.end());
    }
  }
  //std::cout<<"pass one "<<map_it->first<<std::endl;
  
  return solutions;
}
Beispiel #19
0
 PointT ccw(){
     return PointT(-y, x);
 }
 static PointT apply(PointT const &,
                       PointT const &)
 {
   return PointT(0);
 }