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