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); }
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(); }
PointT operator-(){ return PointT(-x, -y); }
PointT operator-(PointT pt){ return PointT(x - pt.x, y - pt.y); }
PointT operator+(PointT pt){ return PointT(x + pt.x, y + pt.y); }
PointT operator/(T coef){ return PointT(x / coef, y / coef); }
PointT operator*(T coef){ return PointT(x * coef, y * coef); }
static PointT apply(PointT, PointT) { return PointT(0,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; }
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; }
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; }
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; }
PointT ccw(){ return PointT(-y, x); }
static PointT apply(PointT const &, PointT const &) { return PointT(0); }