예제 #1
0
  double score(IGMap map, DoubleOrientedPoint p, double[] readings) {
    double s = 0;
    int angleIndex = initialBeamsSkip;
    DoubleOrientedPoint lp = new DoubleOrientedPoint(p.x, p.y, p.theta);
    lp.x += Math.cos(p.theta) * laserPose.x - Math.sin(p.theta) * laserPose.y;
    lp.y += Math.sin(p.theta) * laserPose.x + Math.cos(p.theta) * laserPose.y;
    lp.theta += laserPose.theta;
    int skip = 0;
    double freeDelta = map.getDelta() * freeCellRatio;
    for (int rIndex = initialBeamsSkip; rIndex < readings.length; rIndex++, angleIndex++) {
      skip++;
      skip = skip > likelihoodSkip ? 0 : skip;
      if (skip != 0 || readings[rIndex] > usableRange || readings[rIndex] == 0.0) continue;
      DoublePoint phit = new DoublePoint(lp.x, lp.y);
      phit.x += readings[rIndex] * Math.cos(Utils.theta(lp.theta + laserAngles[angleIndex]));
      phit.y += readings[rIndex] * Math.sin(Utils.theta(lp.theta + laserAngles[angleIndex]));
      IntPoint iphit = map.world2map(phit);
      DoublePoint pfree = new DoublePoint(lp.x, lp.y);
      pfree.x +=
          (readings[rIndex] - map.getDelta() * freeDelta)
              * Math.cos(Utils.theta(lp.theta + laserAngles[angleIndex]));
      pfree.y +=
          (readings[rIndex] - map.getDelta() * freeDelta)
              * Math.sin(Utils.theta(lp.theta + laserAngles[angleIndex]));
      pfree.x = pfree.x - phit.x;
      pfree.y = pfree.y - phit.y;

      IntPoint ipfree = map.world2map(pfree);
      boolean found = false;
      DoublePoint bestMu = new DoublePoint(0., 0.);
      for (int xx = -kernelSize; xx <= kernelSize; xx++) {
        for (int yy = -kernelSize; yy <= kernelSize; yy++) {
          IntPoint pr = new IntPoint(iphit.x + xx, iphit.y + yy);
          IntPoint pf = new IntPoint(pr.x + ipfree.x, pr.y + ipfree.y);
          // int ss = map.getStorage().cellState(pr);
          //                    if ((ss) > 0) {
          PointAccumulator cell = (PointAccumulator) map.cell(pr, true);
          PointAccumulator fcell = (PointAccumulator) map.cell(pf, true);
          if (cell != null && fcell != null) {
            if (cell.doubleValue() > fullnessThreshold && fcell.doubleValue() < fullnessThreshold) {
              DoublePoint mu = DoublePoint.minus(phit, cell.mean());
              if (!found) {
                bestMu = mu;
                found = true;
              } else {
                bestMu = DoublePoint.mulD(mu, mu) < DoublePoint.mulD(bestMu, bestMu) ? mu : bestMu;
              }
            }
          }
          //                    }
        }
      }
      if (found) {
        s += Math.exp(-1. / (gaussianSigma * DoublePoint.mulD(bestMu, bestMu)));
      }
    }
    return s;
  }
예제 #2
0
  public double icpStep(
      DoubleOrientedPoint pret, IGMap map, DoubleOrientedPoint p, double[] readings) {
    int angleIndex = initialBeamsSkip;
    DoubleOrientedPoint lp = new DoubleOrientedPoint(p.x, p.y, p.theta);

    lp.x += Math.cos(p.theta) * laserPose.x - Math.sin(p.theta) * laserPose.y;
    lp.y += Math.sin(p.theta) * laserPose.x + Math.cos(p.theta) * laserPose.y;
    lp.theta += laserPose.theta;
    int skip = 0;
    double freeDelta = map.getDelta() * freeCellRatio;
    List<DoublePointPair> pairs = new ArrayList<DoublePointPair>();

    for (int rIndex = initialBeamsSkip; rIndex < readings.length; rIndex++, angleIndex++) {
      skip++;
      skip = skip > likelihoodSkip ? 0 : skip;
      if (readings[rIndex] > usableRange || readings[rIndex] == 0.0) continue;
      if (skip != 0) continue;
      DoublePoint phit = new DoublePoint(lp.x, lp.y);

      phit.x += readings[rIndex] * Math.cos(lp.theta + laserAngles[angleIndex]);
      phit.y += readings[rIndex] * Math.sin(lp.theta + laserAngles[angleIndex]);
      IntPoint iphit = map.world2map(phit);

      DoublePoint pfree = new DoublePoint(lp.x, lp.y);
      pfree.x +=
          (readings[rIndex] - map.getDelta() * freeDelta)
              * Math.cos(lp.theta + laserAngles[angleIndex]);
      pfree.y +=
          (readings[rIndex] - map.getDelta() * freeDelta)
              * Math.sin(lp.theta + laserAngles[angleIndex]);
      pfree.x = pfree.x - phit.x;
      pfree.y = pfree.y - phit.y;

      IntPoint ipfree = map.world2map(pfree);
      boolean found = false;
      DoublePoint bestMu = new DoublePoint(0., 0.);
      DoublePoint bestCell = new DoublePoint(0., 0.);
      for (int xx = -kernelSize; xx <= kernelSize; xx++)
        for (int yy = -kernelSize; yy <= kernelSize; yy++) {
          IntPoint pr = new IntPoint(iphit.x + xx, iphit.y + yy);
          IntPoint pf = new IntPoint(pr.x + ipfree.x, pr.y + ipfree.y);
          PointAccumulator cell = (PointAccumulator) map.cell(pr, true);
          PointAccumulator fcell = (PointAccumulator) map.cell(pf, true);

          if (cell.doubleValue() > fullnessThreshold && fcell.doubleValue() < fullnessThreshold) {
            DoublePoint mu = DoublePoint.minus(phit, cell.mean());
            if (!found) {
              bestMu = mu;
              bestCell = cell.mean();
              found = true;
            } else if (DoublePoint.mulD(mu, mu) < DoublePoint.mulD(bestMu, bestMu)) {
              bestMu = mu;
              bestCell = cell.mean();
            }
          }
        }
      if (found) {
        pairs.add(new DoublePointPair(phit, bestCell));
      }
    }

    DoubleOrientedPoint result = new DoubleOrientedPoint(0.0, 0.0, 0.0);
    LOG.error("result(" + pairs.size() + ")=" + result.x + " " + result.y + " " + result.theta);
    pret.x = p.x + result.x;
    pret.y = p.y + result.y;
    pret.theta = p.theta + result.theta;
    pret.theta = Math.atan2(Math.sin(pret.theta), Math.cos(pret.theta));
    return score(map, p, readings);
  }