v_localizer_rv.java

利用JAVA编写的群体机器人局部通讯完成一定得队形控制

    if (doneAmbigUpdate) {
      return;
    }

    //this is an ambiguous landmark we've seen...
  
    double currAmbig = 0;
    
    param[0] = robot.getLandmarkDistance(lm);
    param[1] = robot.getLandmarkDistanceVariance(lm);
    param[2] = robot.getLandmarkAngle(lm);
    param[3] = robot.getLandmarkAngleVariance(lm);
    
    for(int k =0;k < numSensorVars; k++) {
      lmUpdater.setMean(k, param[k*2]);
      lmUpdater.setDev(k, param[k*2+1]);
    }

    /*
    for (int j =0; j < ambigLandmarks.length; j++) {
      if (!ambigLandmarks[j]) {
	continue;
      }
      
      lmUpdater.setLocation(getLandmarkLocation(j));
      
           
      start = currAmbig / (double)numAmbigLM;
      start *= (double)numSamples;
      end = (currAmbig+1) / (double)numAmbigLM;
      end *= (double)numSamples;
      
      for (int k = (int)start; k < (int)end; k++) {
	samplesW[k] = lmUpdater.updateSample(samplesX[k], samplesY[k], 
					     samplesT[k], samplesW[k]);
      }
      currAmbig += 1.0;
    }
    */
    
    double best = -999999;
    for (int i =0; i < numSamples; i++) {
      best = -999999;
      
      for (int j =0; j < numAmbigLM; j++) {
	lmUpdater.setLocation(getLandmarkLocation(ambigLMIndex[j]));
	
	samplesW[i] = lmUpdater.updateSample(samplesX[i], samplesY[i],
	 				     samplesT[i], samplesW[i]);
	//	System.out.println("VLOC: UA: samplesW[]="+samplesW[i]+" j="+j);
	if (samplesW[i] > best) {
	  best = samplesW[i];
	  ambigClosest[i] = j;
	  // System.out.println("VLOC: UPA: best="+best+" clos="+j);
	}
      }

      samplesW[i] = best;
    }
    
    samplesAreNormalized = false;

    //now lets generate samples....
    goodSampleProb = normalizeSamples();
    
    if (goodSampleProb >= 0.0)
      System.out.println("VLOC: UA: goodSampProb = "+goodSampleProb);
    double conf = 0.0;
    
    double expect = 0.025; 
    
    expect = Math.pow(expect, 2.0);
    
    double numSensorSamplesReal = (1 - goodSampleProb/(expect*.4))*
      samples.getNumSamples() + 0.5 - 1.0;
    numSensorSamplesReal *= ur.getValue(0.3, 0.8);

    //numSensorSamplesReal /= (double)numAmbigLM;

    currAmbig = 0.0;

    for(int j = 0; j < landmarkSampler.getNumVars(); j++) {
      landmarkSampler.setMean(j,param[j*2]);
      landmarkSampler.setDev(j,param[j*2 + 1]);
    }
    /*
    for (int i =0; i < ambigLMIndex.length; i++) {
    
      landmarkSampler.setPosition(getLandmarkLocation(ambigLMIndex[i]));
      
      System.out.println("VLOC: UA: numSensorSamplesReal (x "+
			 numAmbigLM+" ambig LMS)= "+numSensorSamplesReal);
      if (numSensorSamplesReal > numSamples)
	numSensorSamplesReal = 0;
      
      start = currAmbig / (double)numAmbigLM * (double)numSamples;
      end = (currAmbig+1.0) / (double)numAmbigLM * (double)numSamples;

      for (int j =(int)start; ((double)j) < numSensorSamplesReal; j++) {
	Sample samp = landmarkSampler.generateSample();
	
	robot.clipToMap(samp);
	
	//numCreatedSamples++;
	
	samplesX[j] = samp.data[Sample.x];
	samplesY[j] = samp.data[Sample.y];
	samplesT[j] = samp.data[Sample.t];
	samplesW[j] = samp.data[Sample.w];
      }
      
      currAmbig += 1.0;
    }
    */
    System.out.println("VLOC: UA: numSensorSamplesReal="+numSensorSamplesReal);
    int idx, sidx;
    for (int i =0; i < numSensorSamplesReal; i++) {
      idx = intGen.nextInt() % numAmbigLM;
      if (idx < 0) {
	idx = -idx;
      }
      landmarkSampler.setPosition(getLandmarkLocation(ambigLMIndex[idx]));
      Sample samp = landmarkSampler.generateSample();
      
      robot.clipToMap(samp);
      
      //numCreatedSamples++;
      if (intGen.nextInt()%2 == 0) {
	sidx = i;
      }
      else {
	sidx = (numSamples-1) - i;
      }
      samplesX[sidx] = samp.data[Sample.x];
      samplesY[sidx] = samp.data[Sample.y];
      samplesT[sidx] = samp.data[Sample.t];
      samplesW[sidx] = samp.data[Sample.w];
    }
      

    doneAmbigUpdate = true;
  }   
  
  protected double Vec2Dot(Vec2 a, Vec2 b) {
    return a.x*b.x+a.y*b.y;
  }

  protected void updatePositionLines(long timestamp) {
    int lineChannel = 7;
    //do stuff with lines like we did with landmarks...
    Vec2 [] seenLines = ((LineLocalizationRobot)robot).getVisualLines(timestamp, lineChannel);
    double lengthThreshold = ((Simple)robot).getDictionary().getDouble("LINE_LOCALIZER_LENGTH_THRESHOLD");

    Vec2 tempLine;
    Vec2 dist;
    double slope=-777, b=-777;

    if (seenLines == null)
      return;

    int numSeenLines = 0;
    int numLineSensorVars = 2;
    double minProbGaussianSense = 0.1;
    double norm=-777;
    double [] param = new double[4];
    double angle, theta, psi;
    double seenLength=0;

    for (int i =0; i < seenLines.length; i+= 2) {
      
      if (seenLines[i] == null) {
	//we didnt see this line...
	continue;
      }
  
      ((Simple)robot).setDisplayString("sL "+i);
 
      //get the vector from our position to the closest point on the line we see
      //the line defined by the two vectors seenLines[i] and seenLines[i+1]
      if (seenLines[i].y < 0 && seenLines[i+1].y < 0) {
	Vec2 tmp = (Vec2)seenLines[i].clone();
	seenLines[i] =  (Vec2)seenLines[i+1].clone();
	seenLines[i+1] = tmp;
      }
      
      tempLine = new Vec2(seenLines[i+1]);
      tempLine.sub(seenLines[i]);
      seenLength = tempLine.r;

      if (seenLength < lengthThreshold) {
	//this line is too small so ignore it
	continue;
      }
      
      slope = tempLine.y / tempLine.x;

      if (slope > 9999999) {
	numSeenLines++;

	//it's a vertical line
	double fov = Math.acos(Vec2Dot(seenLines[i], seenLines[i+1]) / 
			       (seenLines[i].r * seenLines[i+1].r));
	
	theta = Math.acos(Vec2Dot(seenLines[i], tempLine) / 
			  (seenLines[i].r*tempLine.r));
	if (theta > Math.PI/2.0) {
	  psi = theta - (0.5*fov) - (Math.PI/2.0);
	}else {
	  fov = SimpleCye.VISION_FOV_RAD;
	  psi = (Math.PI/2.0) - theta + (0.5*fov);
	}
	dist = new Vec2(0,0);
	//dist.setr( seenLines[i].r * Math.asin(psi));
	dist.setr(seenLines[i].x);
	//get the theta!  its the seenLines[i].t + the other part...
	//dist.sett( (Math.PI/2.0) - psi + 0.5*SimpleCye.VISION_FOV_RAD);
	
	dist.sett(Units.ClipRad(psi)); 
	System.out.println("VLOC: UPL: vert: theta="+Units.RadToDeg(theta)+" psi="+Units.RadToDeg(psi)+" fov="+Units.RadToDeg(fov));
	System.out.println("VLOCK UPL: vert: dist.r="+dist.r+" dist.t(deg)="+Units.RadToDeg(dist.t));
       
      } else {

	//forget about it right now...
	
	/*	b = seenLines[i].y - slope*seenLines[i].x;
	
	norm = -b / (1 + slope*slope);
	
	dist = new Vec2(norm*slope, -1*norm);

	dist.t = Units.ClipRad(dist.t);
      
	Vec2 mid = new Vec2(seenLines[i]);
	mid.sub(dist);
	dist.t = Math.tan(dist.r / mid.r) + 0.5* SimpleCye.VISION_FOV_RAD;
	System.out.println("VLOC: UPL: nonvert: dist.r="+dist.r+" dist.t (deg) ="+Units.RadToDeg(dist.t));
	*/
	continue;
      }
    

      System.out.println("VLOC: UPL: line length="+tempLine.r+" b="+b+" norm="+norm+" slope="+slope);
          
      //      lineUpdater.setSeenLine(getLine(i/2).getStart(), getLine(i/2).getEnd());
      //lineUpdater.setSeenLine(dist, angle);
      lineUpdater.setMinProb(minProbGaussianSense);
      
      param[0] = dist.r; //robot.getLineDistance(i); //est distance to line
      param[1] = 0.01; //robot.getLineDistanceVariance(i);
      param[2] = Units.ClipRad(dist.t); //robot.getLineAngle(i); //est angle relative to normal of line
      param[3] = 0.01; //robot.getLineAngleVariance(i);
      
      //we need to set deviances ourselves..
      //param[0] = 0.001; //deviation on the distance to the midpoint of seen line segment
      //param[1] = 0.001; //deviation on the theta of that segment
      
      for(int j =0;j < numLineSensorVars; j++) {
	lineUpdater.setMean(j, param[j*2]);
	lineUpdater.setDev(j, param[j]);
      }
      
      for (int j = 0; j < numSamples; j++) {
	samplesW[j] = lineUpdater.updateSample(samplesX[j], samplesY[j], 
					       samplesT[j], samplesW[j]);
      }
      
      samplesAreNormalized = false;
    }
    
    double goodSampleProb = normalizeSamples();
    
    if (goodSampleProb >= 0.0) {
      System.out.println("VLOC: ULS: goodSampleProb = "+goodSampleProb);
    } 

    if (numSeenLines > 0) {
      double expect = 0.014;
      //      expect = Math.pow(expect, 2.0*numSeenLines);
      double numLineSamplesReal = (1 - goodSampleProb/(expect*0.4))*
	samples.getNumSamples() + 0.5 - 1.0;
      System.out.println("VLOC: UPL: numLineSamplesReal="+numLineSamplesReal);
      //keep the same params from up there (assume 1 line seen FIX)
      

      for (int j =0; j < lineSampler.getNumVars(); j++) {
	lineSampler.setMean(j, param[j*2]);
	lineSampler.setDev(j, param[j*2+1]);
      }

      if (numLineSamplesReal > numSamples) {
	numLineSamplesReal = 0;
      }
     
      lineSampler.setSeenLineLength(seenLength);
      
      for (int j = 0; (double)j < numLineSamplesReal; j++) {
	Sample samp = lineSampler.generateSample();

	//	System.out.println("VLOC: UPL: s.x="+samp.x+" s.y="+samp.y+" s.t="+Units.RadToDeg(samp.t));
	robot.clipToMap(samp);
	
	samplesX[j] = samp.data[Sample.x];
	samplesY[j] = samp.data[Sample.y];
	samplesT[j] = samp.data[Sample.t];
	samplesW[j] = samp.data[Sample.w];
      }
	
    }
  }

  protected LineSim getLine(int i) {
    return allTheLines[i];
  }

    protected Sample getPosition() {
	Sample res;
	//	System.out.println("VLOCRV: getPosition() start");

	//samples.normalizeSamples();
	normalizeSamples();
	
	double[]  mean = new double[Sample.size-1];
	double[] var = new double[mean.length];
		
	mean[0] = mean[1] = mean[2] = 0.0;
	var[0] = var[1] = var[2] = 0.0;
	double x,y,t;

	for (int i = 0; i < numSamples; i++) {
	  x = samplesX[i];
	  y = samplesY[i];
	  t = samplesT[i];

	  mean[0] += x;
	  mean[1] += y;
	  mean[2] += Math.cos(t);

	  //	  var[0] += x*x;
	  //  var[1] += y*y;
	  var[2] += Math.sin(t);
	}
	
	double sum =0.0;


	//calc means for X and Y
	mean[0] /= numSamples;
	mean[1] /= numSamples;
	//find variances for X and Y
	/*	var[0] /= numSamples;
		var[1] /= numSamples;
		var[0] -= mean[0]*mean[0];
		var[1] -= mean[1]*mean[1];
	*/
	double cosMean = mean[2]/numSamples;
	double sinMean = var[2]/numSamples;
	
	mean[2] = Math.atan2(sinMean, cosMean); 
	//var[2] = 1 - Math.sqrt(cosMean*cosMean+sinMean*sinMean);
	/*
	  if (var[2] < 0.0)
	  var[2] = 0.0;
	*/
	res = new Sample(mean[0], mean[1], mean[2]);
	//	System.out.println("VLOCRV: getPosition(): mean is "+res.toString());
	return res;
    }
    
    
    protected void updatePositionMovement() {

        moveUpdateCnt++;
	double [] param = robot.getMovementDistParams();

	for (int i =0; i < numMoveVars; i++) {
	  // 	  System.out.println("VLOC: i = "+i+" UPM: mean = "+param[i*2]+" dev = "+param[i*2+1]);
	    moveUpdater.setMean(i, param[i*2]);
	    
	    moveUpdater.setDev(i, param[i*2+1]);
	}
	

	double [] res;
	for(int i =0; i < numSamples; i++) {
	    //   	    System.out.println("VLOC: UPM: old = "+s.toString());
	    res = moveUpdater.updateSample(samplesX[i], samplesY[i], samplesT[i]);
	   
	    /*
              int onlm = onLandmark(s);
              while (onlm >= 0) {
	      //we're on lm onlm!
	      System.out.println("on landmark! "+onlm);
	      Vec2 lmLoc = getLandmarkLocation(onlm);
	      double t = ur.getValue(0.0, 2*Math.PI);
	      Vec2 adjust = new Vec2(lmLoc.x, lmLoc.y);
	      Vec2 delta = new Vec2(Math.cos(t)*(getLandmarkRadius(onlm)+0.1),
              Math.sin(t)*(getLandmarkRadius(onlm)+0.1));
	      adjust.add(delta);
	      s.data[Sample.x] = adjust.x;
	      s.data[Sample.y] = adjust.y;
	      
	      robot.clipToMap(s);
	      
	      onlm = onLandmark(s);
              }
	    */
	    //FIX:  sample could still end up on LM...
            //robot.clipToMap(s);

	    res  = robot.clipToMap(res[0], res[1], res[2]);
	    
	    samplesX[i] = res[0];
	    samplesY[i] = res[1];
	    samplesT[i] = res[2];
	}
	
    }
 
    protected void resetPosition() {	    
	Sample s;

	System.out.println("VLOCRV: resetPosition() start");

	Range resetX, resetY, resetT;

	//get the map dimensions from the dsc file
	resetX = new Range(((Simple)robot).getDictionary().getString("MAP_RANGE_X"));
 	resetY = new Range(((Simple)robot).getDictionary().getString("MAP_RANGE_Y"));
 	resetT = new Range(((Simple)robot).getDictionary().getString("MAP_RANGE_THETA"));

	//set up the unifrom dist sampler
	us.setRange(0, resetX);
        us.setRange(1, resetY);
        us.setRange(2, resetT);

	//generate uniform dist of samples
	double [] res;
	for (int i =0; i < numSamples; i++) {
	    res = us.generateSampleArray();
	    samplesX[i] = res[0];
	    samplesY[i] = res[1];
	    samplesT[i] = res[2];
	    samplesW[i] =1.0;
	   
	}
	samplesAreNormalized = true;

	//	samples.setNormalized(true);
    }


  public double normalizeSamples() {
    if (samplesAreNormalized == true) 
	    return -1.0;
    
    double [] cumWeights = new double[numSamples+1];
    
    double cumWeight = 0.0;
    for (int i =0; i < numSamples; i++) {
      cumWeights[i] = cumWeight;
      cumWeight+= samplesW[i];
    }
    cumWeights[numSamples] = cumWeight;
    
    //	System.out.println("SS: normalize: cumWeight = "+cumWeight);
    
    //here we resample from the samples....
    for (int i= 0; i < numSamples; i++) {
      double cumWeightToFind = ur.getValue(0.0, cumWeight);
      
      int low, hi, mid;
      
      low = 0;
      hi = numSamples-1;
      
      while (low < hi-1) {
	mid = (low + hi) / 2;
	
	if (cumWeightToFind < cumWeights[mid]) 
	  hi = mid;
	else 
	  low = mid;
      }
      
     
      newSamplesX[i] = samplesX[low];
      newSamplesY[i] = samplesY[low];
      newSamplesT[i] = samplesT[low];
      samplesW[i] = 1.0;
    }
    
    /*
      Sample []tmp = samples;
      samples = newSamples;
      newSamples = tmp;
    */
    samplesX = newSamplesX;
    samplesY = newSamplesY;
    samplesT = newSamplesT;

    samplesAreNormalized = true;

    System.out.println("cumWeight = "+cumWeight);
    return cumWeight/numSamples;
  }
  
    protected int onLandmark(Sample s) {
        for (int i =0; i < landmarkAreas.length; i++) {
            if (landmarkAreas[i].contains(s.data[Sample.x], s.data[Sample.y])) {
                return i;
            }
        }
        return -1;
    }

    protected Vec2 getLandmarkLocation(int lm) {
        return landmarks[lm].getPosition();
    }
  
    protected double getLandmarkRadius(int lm) {
        return ((LandmarkSim)landmarks[lm]).getRadius();
    }
}