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 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); }
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 double registerScan(IGMap map, DoubleOrientedPoint p, double[] readings) { if (!m_activeAreaComputed) computeActiveArea(map, p, readings); // this operation replicates the cells that will be changed in the registration operation // map.allocActiveArea(); 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; IntPoint p0 = map.world2map(lp); int readingIndex; int angleIndex = initialBeamsSkip; double esum = 0; for (readingIndex = initialBeamsSkip; readingIndex < laserBeams; readingIndex++, angleIndex++) { if (generateMap) { double d = readings[readingIndex]; if (d >= laserMaxRange) continue; if (d >= usableRange) d = usableRange; DoublePoint phit = new DoublePoint( d * Math.cos(Utils.theta(lp.theta + laserAngles[angleIndex])) + lp.x, d * Math.sin(Utils.theta(lp.theta + laserAngles[angleIndex])) + lp.y); IntPoint p1 = map.world2map(phit); d += map.getDelta(); GridLineTraversalLine line = new GridLineTraversalLine(); line.points = m_linePoints; GridLineTraversalLine.gridLine(p0, p1, line); for (int i = 0; i < line.numPoints - 1; i++) { PointAccumulator pa = (PointAccumulator) map.cell(line.points[i], false); if (pa != null) { double e = -pa.entropy(); pa.update(false, new DoublePoint(0.0, 0.0)); e += pa.entropy(); esum += e; } } if (d < usableRange) { PointAccumulator pa = (PointAccumulator) map.cell(p1, false); if (pa != null) { double e = -pa.entropy(); pa.update(true, phit); e += pa.entropy(); esum += e; } } } else { double r = readings[readingIndex]; if (r >= laserMaxRange || r >= usableRange || r == 0) { continue; } // DoublePoint phit = new DoublePoint(lp.x, lp.y); double phitx = lp.x; double phity = lp.y; phitx += r * Math.cos(Utils.theta(lp.theta + laserAngles[angleIndex])); phity += r * Math.sin(Utils.theta(lp.theta + laserAngles[angleIndex])); IntPoint p1 = map.world2map(phitx, phity); PointAccumulator pa = (PointAccumulator) map.cell(p1, false); if (pa != null) { pa.update(true, phitx, phity); } } } return esum; }