Exemplo n.º 1
0
  public LikeliHood likelihood(IGMap map, DoubleOrientedPoint p, double[] readings) {
    List<ScoredMove> moveList = new ArrayList<ScoredMove>();

    for (double xx = -llsamplerange; xx <= llsamplerange; xx += llsamplestep) {
      for (double yy = -llsamplerange; yy <= llsamplerange; yy += llsamplestep) {
        for (double tt = -lasamplerange; tt <= lasamplerange; tt += lasamplestep) {
          DoubleOrientedPoint rp = new DoubleOrientedPoint(p.x, p.y, p.theta);
          rp.x += xx;
          rp.y += yy;
          rp.theta += tt;

          ScoredMove sm = new ScoredMove();
          sm.pose = rp;

          LikeliHoodAndScore ls = likelihoodAndScore(map, rp, readings);
          sm.score = ls.s;
          sm.likelihood = ls.l;

          moveList.add(sm);
        }
      }
    }

    // normalize the likelihood
    double lmax = -1e9;
    double lcum = 0;
    for (ScoredMove it : moveList) {
      lmax = it.likelihood > lmax ? it.likelihood : lmax;
    }
    for (ScoredMove it : moveList) {
      lcum += Math.exp(it.likelihood - lmax);
      it.likelihood = Math.exp(it.likelihood - lmax);
    }

    DoubleOrientedPoint mean = new DoubleOrientedPoint(0.0, 0.0, 0.0);
    for (ScoredMove it : moveList) {
      double x = mean.x + it.pose.x * it.likelihood;
      double y = mean.y + it.pose.y * it.likelihood;
      double theta = mean.theta + it.pose.theta * it.likelihood;
      mean.x = x;
      mean.y = y;
      mean.theta = theta;
    }
    mean.x = mean.x * (1. / lcum);
    mean.y = mean.y * (1. / lcum);
    mean.theta = mean.theta * (1. / lcum);

    Covariance3 cov = new Covariance3(0., 0., 0., 0., 0., 0.);
    for (ScoredMove it : moveList) {
      DoubleOrientedPoint delta = DoubleOrientedPoint.minus(it.pose, mean);
      delta.theta = Math.atan2(Math.sin(delta.theta), Math.cos(delta.theta));
      cov.xx += delta.x * delta.x * it.likelihood;
      cov.yy += delta.y * delta.y * it.likelihood;
      cov.tt += delta.theta * delta.theta * it.likelihood;
      cov.xy += delta.x * delta.y * it.likelihood;
      cov.xt += delta.x * delta.theta * it.likelihood;
      cov.yt += delta.y * delta.theta * it.likelihood;
    }
    cov.xx /= lcum;
    cov.xy /= lcum;
    cov.xt /= lcum;
    cov.yy /= lcum;
    cov.yt /= lcum;
    cov.tt /= lcum;

    return new LikeliHood(lmax, mean, cov, Math.log(lcum));
  }
Exemplo n.º 2
0
  public double optimize(
      DoubleOrientedPoint _mean,
      Covariance3 _cov,
      IGMap map,
      DoubleOrientedPoint init,
      double[] readings) {
    List<ScoredMove> moveList = new ArrayList<ScoredMove>();
    double bestScore = -1;
    DoubleOrientedPoint currentPose = init;
    ScoredMove sm = new ScoredMove(currentPose, 0, 0);
    LikeliHoodAndScore ls = likelihoodAndScore(map, currentPose, readings);
    sm.likelihood = ls.l;
    sm.score = ls.s;
    double currentScore = sm.score;
    moveList.add(sm);
    double adelta = optAngularDelta, ldelta = optLinearDelta;
    int refinement = 0;

    do {
      if (bestScore >= currentScore) {
        refinement++;
        adelta *= .5;
        ldelta *= .5;
      }
      bestScore = currentScore;
      DoubleOrientedPoint bestLocalPose = currentPose;
      DoubleOrientedPoint localPose = currentPose;

      Move move = Move.Front;
      do {
        localPose = currentPose;
        switch (move) {
          case Front:
            localPose.x += ldelta;
            move = Move.Back;
            break;
          case Back:
            localPose.x -= ldelta;
            move = Move.Left;
            break;
          case Left:
            localPose.y -= ldelta;
            move = Move.Right;
            break;
          case Right:
            localPose.y += ldelta;
            move = Move.TurnLeft;
            break;
          case TurnLeft:
            localPose.theta += adelta;
            move = Move.TurnRight;
            break;
          case TurnRight:
            localPose.theta -= adelta;
            move = Move.Done;
            break;
          default:
        }
        double localScore = 0, localLikelihood = 0;
        // update the score
        ls = likelihoodAndScore(map, localPose, readings);
        localLikelihood = ls.s;
        localLikelihood = ls.l;
        if (localScore > currentScore) {
          currentScore = localScore;
          bestLocalPose = localPose;
        }
        sm.score = localScore;
        sm.likelihood = localLikelihood;
        sm.pose = localPose;
        moveList.add(sm);
        // update the move list
      } while (move != Move.Done);
      currentPose = bestLocalPose;
      // here we look for the best move;
    } while (currentScore > bestScore || refinement < optRecursiveIterations);

    // normalize the likelihood
    double lmin = 1e9;
    double lmax = -1e9;
    for (ScoredMove it : moveList) {
      lmin = it.likelihood < lmin ? it.likelihood : lmin;
      lmax = it.likelihood > lmax ? it.likelihood : lmax;
    }
    for (ScoredMove it : moveList) {
      it.likelihood = Math.exp(it.likelihood - lmax);
    }
    // compute the mean
    DoubleOrientedPoint mean = new DoubleOrientedPoint(0.0, 0.0, 0.0);
    double lacc = 0;
    for (ScoredMove it : moveList) {
      mean = DoubleOrientedPoint.plus(mean, DoubleOrientedPoint.mulN(it.pose, it.likelihood));
      lacc += it.likelihood;
    }
    mean = DoubleOrientedPoint.mulN(mean, (1. / lacc));
    Covariance3 cov = new Covariance3(0., 0., 0., 0., 0., 0.);
    for (ScoredMove it : moveList) {
      DoubleOrientedPoint delta = DoubleOrientedPoint.minus(it.pose, mean);
      delta.theta = Math.atan2(Math.sin(delta.theta), Math.cos(delta.theta));
      cov.xx += delta.x * delta.x * it.likelihood;
      cov.yy += delta.y * delta.y * it.likelihood;
      cov.tt += delta.theta * delta.theta * it.likelihood;
      cov.xy += delta.x * delta.y * it.likelihood;
      cov.xt += delta.x * delta.theta * it.likelihood;
      cov.yt += delta.y * delta.theta * it.likelihood;
    }
    cov.xx /= lacc;
    cov.xy /= lacc;
    cov.xt /= lacc;
    cov.yy /= lacc;
    cov.yt /= lacc;
    cov.tt /= lacc;

    _mean = currentPose;
    _cov = cov;
    return bestScore;
  }