public void drawPeopleOld(int num, int divs) {
// if(divs == 2) {
// divs = 4;
// }
double theta = calculateTheta(divs);
double offset = theta / 2;
boolean quarter = true;
DoublePoint center = getCenter();
for (int i = 0; i < num; i++) {
double r = calculatePersonDistance(i / divs);
if (divs <= 3 && i % divs == 0) {
if (quarter) {
offset = theta / 4;
} else {
offset = (3 * theta) / 4;
}
quarter = !quarter;
}
if (divs <= 3) {
r = calculatePersonDistance(i / (divs * 2));
}
double angle = theta * i + offset;
DoublePoint pos = getPolarProjectedPoint(center, r, angle);
doAdd((int) pos.getX(), (int) pos.getY());
}
repaint();
}
private void createSpline(Point start, Point end) {
int distance = (int) Point2D.distance(start.x, start.y, end.x, end.y);
n = distance / 100 + random.nextInt(3) + 4;
if (distance < 100) n = 3;
points = new DoublePoint[n];
points[0] = new DoublePoint(start.x, start.y);
points[n - 1] = new DoublePoint(end.x, end.y);
int midPoints = n - 2;
DoublePoint lastPos = new DoublePoint(points[0].x, points[0].y);
for (int i = 1; i < n - 1; i++) {
double X = lastPos.x;
double Y = lastPos.y;
double spacing = distance / (midPoints + 2);
int randomNum = random.nextInt(2);
if (randomNum == 0) X += createSmartControlPoint(i, spacing, false, distance);
else X -= createSmartControlPoint(i, spacing, false, distance);
randomNum = random.nextInt(2);
if (randomNum == 0) Y += createSmartControlPoint(i, spacing, true, distance);
else Y -= createSmartControlPoint(i, spacing, true, distance);
points[i] = new DoublePoint(X, Y);
lastPos.x = X;
lastPos.y = Y;
}
generateSpline();
}
public void addAManip(int ithPerson, double theta, DoublePoint center, int divs) {
DoublePoint pos = null;
while (true) {
double offset = calculateOffsetAngle(theta);
double r = calculatePersonDistance2(ithPerson / divs);
double angle = theta * ithPerson + offset;
pos = getPolarProjectedPoint(center, r, angle);
if (findDotArea((int) pos.getX(), (int) pos.getY()) == null) {
break;
}
}
doAdd((int) pos.getX(), (int) pos.getY());
}
/**
* Utility -- does a repaint rect around a region specified by two points. Used by smart repaint
* when dragging a dot.
*/
private void repaintRegion(DoublePoint start, DoublePoint end) {
int smallX = Math.min((int) start.getX(), (int) end.getX());
int smallY = Math.min((int) start.getY(), (int) end.getY());
int width = (int) Math.abs(end.getX() - start.getX());
int height = (int) Math.abs(end.getY() - start.getY());
repaint(smallX - 2, smallY - 2, width + 5, height + 5);
}
private void moveLine(Line l, DoublePoint latestPoint) {
DoublePoint origEndPoint = l.getEnd();
DoublePoint origStartPoint = l.getStart();
// repaintRegion(origStartPoint, origEndPoint);
repaint();
int dx = (int) (latestPoint.getX() - origPoint.getX());
int dy = (int) (latestPoint.getY() - origPoint.getY());
origPoint = latestPoint;
l.moveBy(dx, dy);
DoublePoint newStartPoint = l.getStart();
DoublePoint newEndPoint = l.getEnd();
repaintRegion(newStartPoint, newEndPoint);
repaint();
}
// Moves x,y both the given dx,dy
public void moveBy(int dx, int dy) {
startPoint.setLocation(startPoint.getX() + dx, startPoint.getY() + dy);
endPoint.setLocation(endPoint.getX() + dx, endPoint.getY() + dy);
}
public double getHeight() {
return Math.abs(startPoint.getY() - endPoint.getY());
}
public double getWidth() {
return Math.abs(startPoint.getX() - endPoint.getX());
}
public double getY1() {
return Math.min(startPoint.getY(), endPoint.getY());
}
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 void computeActiveArea(IGMap map, DoubleOrientedPoint p, double[] readings) {
if (m_activeAreaComputed) return;
// Set<IntPoint> activeArea = new HashSet<IntPoint>();
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);
DoublePoint min = map.map2world(new IntPoint(0, 0));
DoublePoint max = map.map2world(new IntPoint(map.getMapSizeX() - 1, map.getMapSizeY() - 1));
if (lp.x < min.x) min.x = lp.x;
if (lp.y < min.y) min.y = lp.y;
if (lp.x > max.x) max.x = lp.x;
if (lp.y > max.y) max.y = lp.y;
int readingIndex;
int angleIndex = initialBeamsSkip;
for (readingIndex = initialBeamsSkip; readingIndex < laserBeams; readingIndex++, angleIndex++) {
double r = readings[readingIndex];
double angle = laserAngles[angleIndex];
if (r > laserMaxRange || r == 0.0 || r > Double.MAX_VALUE) continue;
double d = r > usableRange ? usableRange : r;
// DoublePoint phit = new DoublePoint(lp.x, lp.y);
double phitx = lp.x, phity = lp.y;
phitx += d * Math.cos(lp.theta + angle);
phity += d * Math.sin(lp.theta + angle);
if (phitx < min.x) min.x = phitx;
if (phity < min.y) min.y = phity;
if (phitx > max.x) max.x = phitx;
if (phity > max.y) max.y = phity;
}
if (!map.isInsideD(min) || !map.isInsideD(max)) {
DoublePoint lmin = map.map2world(new IntPoint(0, 0));
DoublePoint lmax = map.map2world(new IntPoint(map.getMapSizeX() - 1, map.getMapSizeY() - 1));
min.x = (min.x >= lmin.x) ? lmin.x : min.x - enlargeStep;
max.x = (max.x <= lmax.x) ? lmax.x : max.x + enlargeStep;
min.y = (min.y >= lmin.y) ? lmin.y : min.y - enlargeStep;
max.y = (max.y <= lmax.y) ? lmax.y : max.y + enlargeStep;
map.resize(min.x, min.y, max.x, max.y);
}
// readingIndex = initialBeamsSkip;
// angleIndex = initialBeamsSkip;
// for (readingIndex = initialBeamsSkip; readingIndex < laserBeams; readingIndex++,
// angleIndex++) {
// if (generateMap) {
// double d = readings[readingIndex];
// if (d > laserMaxRange || d == 0.0 || d > Double.MAX_VALUE)
// continue;
// if (d > usableRange)
// d = usableRange;
//
// DoublePoint phit = new DoublePoint(d * Math.cos(lp.theta +
// laserAngles[angleIndex]) + lp.x, d * Math.sin(lp.theta + laserAngles[angleIndex]) + lp.y);
// p0 = map.world2map(lp);
// 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++) {
// IntPoint patch = (m_linePoints[i]);
// activeArea.add(patch);
// }
// if (d <= usableRange) {
// IntPoint patch = (p1);
// activeArea.add(patch);
// }
// } else {
// double r = readings[readingIndex];
// double angle = laserAngles[angleIndex];
// if (readings[readingIndex] > laserMaxRange || readings[readingIndex] >
// usableRange) {
// continue;
// }
//// DoublePoint phit = new DoublePoint(lp.x, lp.y);
// double phitx = lp.x;
// double phity = lp.y;
// phitx += r * Math.cos(lp.theta + angle);
// phity += r * Math.sin(lp.theta + angle);
//
// IntPoint p1 = map.world2map(phitx, phity);
// assert (p1.x >= 0 && p1.y >= 0);
// IntPoint cp = (p1);
// assert (cp.x >= 0 && cp.y >= 0);
// activeArea.add(cp);
// }
// }
// this allocates the unallocated cells in the active area of the map
// map.setActiveArea(activeArea, false);
m_activeAreaComputed = true;
}
private DoublePoint getPolarProjectedPoint(DoublePoint orig, double r, double theta) {
double x = r * Math.sin(Math.toRadians(theta));
double y = r * Math.cos(Math.toRadians(theta));
return new DoublePoint((orig.getX() + x), (orig.getY() + y));
}
