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));
}
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;
}