C++ ConvexPolygon::seed方法代码示例

void ConstraintBuilder::processEnvironment() {

    //pre-compute allowed positions, max and min thetas
    map->precomputeAllowedPositions(allowedStates,stateIntervals);

    double theta_increment = 2*M_PI / (double) N_THETA_INCREMENTS;
    double len = 0.5;
    cv::Point pixel;
    size_t x,y;

    //go through all poses in the trajectory
    for( size_t i=0; i<trajectory.size(); i++) {
	//create a polygon with a seed set to the trajectory point
	ConvexPolygon poly;
	poly.seed(0) = trajectory[i].poseX;
	poly.seed(1) = trajectory[i].poseY;
	
	map->world2pixel(poly.seed, pixel);
	if(pixel.x < 0 || pixel.y < 0 || pixel.x >= map->sizeY || pixel.y>= map->sizeX) {
	    cout<<"ERROR in polygon creation - trajectory goes out of map at "<<poly.seed.transpose()<<"\n";
	    return;
	}
	x = pixel.y;
	y = pixel.x;
	cout<<"polygon seed at "<<x<<" "<<y<<endl;

	if(!allowedStates(x,y)) {
	    cout<<"ERROR in polygon creation - trajectory goes through obstacle space at "<<poly.seed.transpose()<<"\n";
	    return;
	}

	int idx = trajectory[i].poseT / theta_increment;	
	if(!stateIntervals(x,y).allowed_theta_phi[idx][0]) {
	    cout<<"ERROR in polygon creation - trajectory goes through obstacle space at "
		<<poly.seed.transpose()<<" with theta wrong at "<<idx<<" \n";
	    return;
	}
	size_t idxmin = (idx == 0) ? 0 : idx-1;
	size_t idxmax = (idx == N_THETA_INCREMENTS-1) ? idx : idx+1;
	
	int nMoves = 0;
	vector<pair<size_t,size_t> > best_endpoints, current_endpoints, eps;
	pair<size_t, size_t> ul (x-5,y-5), ll(x-5,y+5), lr(x+5,y+5), ur(x+5,y-5);
	int cl1, cl2;
	double mindist, mindist2, dist;
	Eigen::Vector2d vec;
	size_t xprev, yprev;
	int MAX_XDIST = 10, MAX_YDIST = 10;

	best_endpoints.push_back(ul);
	best_endpoints.push_back(ll);
	best_endpoints.push_back(lr);
	best_endpoints.push_back(ur);
	
	vector<int> indexes_to_consider;
	if(i>=1) {
	    indexes_to_consider.push_back(i-1);
	}
	//search direction towards next
	if(i<trajectory.size()-1) {
	    indexes_to_consider.push_back(i+1);
	}
	
	while (nMoves<50) {
	    current_endpoints = best_endpoints;
	    //search direction towards previous
	    for(int sn =0; sn<indexes_to_consider.size(); sn++) {
		int next = indexes_to_consider[sn];
		vec<<trajectory[next].poseX,trajectory[next].poseY;
		map->world2pixel(vec, pixel);
		xprev = pixel.y;
		yprev = pixel.x;
		//find the two closest points in best endpoints
		mindist=INT_MAX;
		for(int q =0; q<current_endpoints.size(); q++) {
		    dist = sqrt( pow((double)current_endpoints[q].first-xprev,2) 
			         + pow((double)current_endpoints[q].second-yprev,2));
		    //cout<<"q "<<q<<" dist "<<dist<<" to ("<<xprev<<","<<yprev<<")\n";
		    if(dist<mindist) {
			mindist = dist;
			cl1 = q;
		    }	
		}
		int prev_cl1, next_cl1;
		prev_cl1 = cl1 == 0 ? current_endpoints.size()-1 : cl1-1;
		next_cl1 = cl1 == current_endpoints.size()-1 ? 0 : cl1+1;
		double dist1 = sqrt( pow((double)current_endpoints[prev_cl1].first-xprev,2) 
			+ pow((double)current_endpoints[prev_cl1].second-yprev,2));
		double dist2 = sqrt( pow((double)current_endpoints[next_cl1].first-xprev,2) 
			+ pow((double)current_endpoints[next_cl1].second-yprev,2));

		cl2 = dist1 < dist2 ? prev_cl1 : next_cl1; 

		//move them in the direction of xprev yprev
		int dx1,dy1,dx2,dy2,dx3,dy3;
		dx1 = current_endpoints[cl1].first - xprev;// ? -1 : current_endpoints[cl1].first < xprev ?  1 : 0;
		dy1 = current_endpoints[cl1].second - yprev;// ? -1 : current_endpoints[cl1].second < yprev ?  1 : 0;
		dx2 = current_endpoints[cl2].first - xprev;// ? -1 : current_endpoints[cl2].first < xprev ?  1 : 0;
		dy2 = current_endpoints[cl2].second - yprev;// ? -1 : current_endpoints[cl2].second < yprev ?  1 : 0;
		
//.........这里部分代码省略.........