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;
}
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);
}
public LikeliHoodAndScore likelihoodAndScore(
IGMap map, DoubleOrientedPoint p, double[] readings) {
double l = 0;
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;
double noHit = nullLikelihood / (likelihoodSigma);
int skip = 0;
int c = 0;
PointAccumulator emptyPointAccumulator = new PointAccumulator();
double freeDelta = map.getDelta() * freeCellRatio;
for (int rIndex = initialBeamsSkip; rIndex < readings.length; rIndex++, angleIndex++) {
skip++;
skip = skip > likelihoodSkip ? 0 : skip;
if (readings[rIndex] > usableRange) {
continue;
}
if (skip != 0) {
continue;
}
double phitx = lp.x;
double phity = lp.y;
phitx += readings[rIndex] * Math.cos(Utils.theta(lp.theta + laserAngles[angleIndex]));
phity += readings[rIndex] * Math.sin(Utils.theta(lp.theta + laserAngles[angleIndex]));
// IntPoint iphit = map.world2map(phitx, phity);
int iphitx = (int) Math.round((phitx - map.getCenter().x) / map.getDelta()) + map.getSizeX2();
int iphity = (int) Math.round((phity - map.getCenter().y) / map.getDelta()) + map.getSizeY2();
// DoublePoint pfree = new DoublePoint(lp.x, lp.y);
double pfreex = lp.x;
double pfreey = lp.y;
pfreex +=
(readings[rIndex] - freeDelta)
* Math.cos(Utils.theta(lp.theta + laserAngles[angleIndex]));
pfreey +=
(readings[rIndex] - freeDelta)
* Math.sin(Utils.theta(lp.theta + laserAngles[angleIndex]));
pfreex = pfreex - phitx;
pfreey = pfreey - phity;
// IntPoint ipfree = map.world2map(pfreex, pfreey);
int ipfreex =
(int) Math.round((pfreex - map.getCenter().x) / map.getDelta()) + map.getSizeX2();
int ipfreey =
(int) Math.round((pfreey - map.getCenter().y) / map.getDelta()) + map.getSizeY2();
boolean found = false;
// DoublePoint bestMu = new DoublePoint(0.0, 0.0);
double bestMux = 0.0;
double bestMuy = 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);
int prx = iphitx + xx;
int pry = iphity + yy;
// IntPoint pf = new IntPoint(pr.x + ipfree.x, pr.y + ipfree.y);
int pfx = prx + ipfreex;
int pfy = pry + ipfreey;
// AccessibilityState s=map.storage().cellState(pr);
// if (s&Inside && s&Allocated){
PointAccumulator cell = (PointAccumulator) map.cell(prx, pry, true);
PointAccumulator fcell = (PointAccumulator) map.cell(pfx, pfy, true);
if (cell == null) {
cell = emptyPointAccumulator;
}
if (fcell == null) {
fcell = emptyPointAccumulator;
}
if (cell.doubleValue() > fullnessThreshold && fcell.doubleValue() < fullnessThreshold) {
double meanx = cell.accx / cell.n;
double meany = cell.accy / cell.n;
double mux = phitx - meanx;
double muy = phity - meany;
// DoublePoint mu = DoublePoint.minus(phit, cell.mean());
if (!found) {
bestMux = mux;
bestMuy = muy;
found = true;
} else {
double mulMu = mux * mux + muy * muy;
double mulBestMu = bestMux * bestMux + bestMuy * bestMuy;
if (mulMu < mulBestMu) {
bestMux = mux;
bestMuy = muy;
}
}
}
}
}
double mulBestMu = bestMux * bestMux + bestMuy * bestMuy;
if (found) {
s += Math.exp(-1.0 / gaussianSigma * mulBestMu);
c++;
}
double f = (1. / likelihoodSigma) * (mulBestMu);
l += (found) ? f : noHit;
}
return new LikeliHoodAndScore(s, l, c);
}
