static void handleKeyTyped(KeyEvent e) {
if (e.getKeyChar() == ' ') {
currentSequenceNumDisplay++;
scribbles = Collections.synchronizedList(new ArrayList<DrawObject>());
currentScribbleNum = 0;
}
drawArea.repaint();
}
/**
* Connects the figure to the new connectableConnector. If there is no new connectableConnector
* the connection reverts to its original one.
*/
public void trackEnd(Point anchor, Point lead, int modifiersEx) {
ConnectionFigure f = getOwner();
// Change node type
if ((modifiersEx
& (InputEvent.META_DOWN_MASK
| InputEvent.CTRL_DOWN_MASK
| InputEvent.ALT_DOWN_MASK
| InputEvent.SHIFT_DOWN_MASK))
!= 0
&& (modifiersEx & InputEvent.BUTTON2_DOWN_MASK) == 0) {
f.willChange();
int index = getBezierNodeIndex();
BezierPath.Node v = f.getNode(index);
if (index > 0 && index < f.getNodeCount()) {
v.mask = (v.mask + 3) % 4;
} else if (index == 0) {
v.mask = ((v.mask & BezierPath.C2_MASK) == 0) ? BezierPath.C2_MASK : 0;
} else {
v.mask = ((v.mask & BezierPath.C1_MASK) == 0) ? BezierPath.C1_MASK : 0;
}
f.setNode(index, v);
f.changed();
fireHandleRequestSecondaryHandles();
}
Point2D.Double p = view.viewToDrawing(lead);
view.getConstrainer().constrainPoint(p);
Connector target = findConnectionTarget(p, view.getDrawing());
if (target == null) {
target = savedTarget;
}
setLocation(p);
if (target != savedTarget) {
disconnect();
connect(target);
}
getOwner().setLiner(savedLiner);
getOwner().updateConnection();
connectableConnector = null;
connectors = Collections.emptyList();
}
public static SymbolAxis getBLMAxis(
final String param,
final Map<String, TypeDataSet> typeSeries,
Map<String, NumberAxis> typeAxes) {
final ArrayList<LossDetector> detectors = new ArrayList<LossDetector>();
for (TypeDataSet v : typeSeries.values()) {
if (v.isVisible()) {
detectors.addAll(v.getDetectors());
}
}
if (detectors.size() > 0) {
Collections.sort(detectors, LossDetector.getComparator());
}
String[] names = new String[detectors.size()];
final LossDetector[] dets = new LossDetector[detectors.size()];
final ValueAxis[] axes = new ValueAxis[detectors.size()];
int index = 0;
for (LossDetector det : detectors) {
names[index] = det.getShortName();
dets[index] = det;
axes[index] = typeAxes.get(det.getType());
index++;
}
@SuppressWarnings("rawtypes")
SymbolAxis axis =
new SymbolAxis("", names) {
private static final long serialVersionUID = -2911592900608877261L;
protected void drawGridBandsHorizontal(
Graphics2D g2,
Rectangle2D plotArea,
Rectangle2D dataArea,
boolean firstGridBandIsDark,
List ticks) {
boolean currentGridBandIsDark = firstGridBandIsDark;
double yy = dataArea.getY();
double xx1, xx2;
// gets the outline stroke width of the plot
double outlineStrokeWidth;
if (getPlot().getOutlineStroke() != null) {
outlineStrokeWidth = ((BasicStroke) getPlot().getOutlineStroke()).getLineWidth();
} else {
outlineStrokeWidth = 1d;
}
Iterator iterator = ticks.iterator();
ValueTick tick;
Rectangle2D band;
while (iterator.hasNext()) {
tick = (ValueTick) iterator.next();
int index = (int) tick.getValue();
xx1 = valueToJava2D(tick.getValue() - 0.5d, dataArea, RectangleEdge.BOTTOM);
xx2 = valueToJava2D(tick.getValue() + 0.5d, dataArea, RectangleEdge.BOTTOM);
currentGridBandIsDark = !currentGridBandIsDark;
if (dets.length > 0) {
LossDetector detector = dets[index];
Set<String> normalization = typeSeries.get(detector.getType()).getNormalization();
boolean limitVisibility = typeSeries.get(detector.getType()).isLimitVisible();
if (limitVisibility) {
ScalarSignalValue limit =
(ScalarSignalValue) detector.getValue(param, normalization);
if (limit != null) {
ValueAxis ax = axes[index];
double losslimit = Math.abs(limit.getValue());
double zz = ax.valueToJava2D(losslimit, dataArea, RectangleEdge.RIGHT);
g2.setPaint(Color.red);
Rectangle2D.Double zRect =
new Rectangle2D.Double(
xx1, yy + outlineStrokeWidth, xx2 - xx1, zz - yy - outlineStrokeWidth);
g2.fill(zRect);
}
}
if (STATUS_DECORATION) {
int status = detectors.get((int) tick.getValue()).getStatus();
Color color = statusColors.get(status);
if (color == null || status == LossDetector.STATUS_OK) {
} else {
g2.setPaint(color);
double x, y, w, h;
x = xx1;
y = yy + outlineStrokeWidth;
w = xx2 - xx1;
h = 10;
band = new Rectangle2D.Double(x, y, w, 10);
g2.fill(band);
}
}
}
}
g2.setPaintMode();
}
protected AxisState drawTickMarksAndLabels(
Graphics2D g2,
double cursor,
Rectangle2D plotArea,
Rectangle2D dataArea,
RectangleEdge edge) {
AxisState state = new AxisState(cursor);
if (isAxisLineVisible()) {
drawAxisLine(g2, cursor, dataArea, edge);
}
double ol = getTickMarkOutsideLength();
double il = getTickMarkInsideLength();
List ticks = refreshTicks(g2, state, dataArea, edge);
state.setTicks(ticks);
g2.setFont(getTickLabelFont());
Iterator iterator = ticks.iterator();
while (iterator.hasNext()) {
ValueTick tick = (ValueTick) iterator.next();
if (isTickLabelsVisible()) {
/// status decorations
if (STATUS_DECORATION && dets.length > 0) {
int status = detectors.get((int) tick.getValue()).getStatus();
Color color = statusColors.get(status);
if (color == null) {
g2.setPaint(getTickLabelPaint());
} else {
g2.setPaint(color);
}
} else {
g2.setPaint(getTickLabelPaint());
}
float[] anchorPoint = calculateAnchorPoint(tick, cursor, dataArea, edge);
TextUtilities.drawRotatedString(
tick.getText(),
g2,
anchorPoint[0],
anchorPoint[1],
tick.getTextAnchor(),
tick.getAngle(),
tick.getRotationAnchor());
}
if (isTickMarksVisible()) {
float xx = (float) valueToJava2D(tick.getValue(), dataArea, edge);
Line2D mark = null;
g2.setStroke(getTickMarkStroke());
g2.setPaint(getTickMarkPaint());
if (edge == RectangleEdge.LEFT) {
mark = new Line2D.Double(cursor - ol, xx, cursor + il, xx);
} else if (edge == RectangleEdge.RIGHT) {
mark = new Line2D.Double(cursor + ol, xx, cursor - il, xx);
} else if (edge == RectangleEdge.TOP) {
mark = new Line2D.Double(xx, cursor - ol, xx, cursor + il);
} else if (edge == RectangleEdge.BOTTOM) {
mark = new Line2D.Double(xx, cursor + ol, xx, cursor - il);
}
g2.draw(mark);
}
}
// need to work out the space used by the tick labels...
// so we can update the cursor...
double used = 0.0;
if (isTickLabelsVisible()) {
if (edge == RectangleEdge.LEFT) {
used += findMaximumTickLabelWidth(ticks, g2, plotArea, isVerticalTickLabels());
state.cursorLeft(used);
} else if (edge == RectangleEdge.RIGHT) {
used = findMaximumTickLabelWidth(ticks, g2, plotArea, isVerticalTickLabels());
state.cursorRight(used);
} else if (edge == RectangleEdge.TOP) {
used = findMaximumTickLabelHeight(ticks, g2, plotArea, isVerticalTickLabels());
state.cursorUp(used);
} else if (edge == RectangleEdge.BOTTOM) {
used = findMaximumTickLabelHeight(ticks, g2, plotArea, isVerticalTickLabels());
state.cursorDown(used);
}
}
return state;
}
};
// axis.setTickLabelFont(axis.getTickLabelFont().deriveFont(Font.BOLD, 14.0f));
return axis;
}
static {
points = Collections.synchronizedList(new ArrayList<DrawObject>());
lines = Collections.synchronizedList(new ArrayList<DrawObject>());
ovals = Collections.synchronizedList(new ArrayList<DrawObject>());
rectangles = Collections.synchronizedList(new ArrayList<DrawObject>());
images = Collections.synchronizedList(new ArrayList<DrawObject>());
labels = Collections.synchronizedList(new ArrayList<DrawObject>());
scribbles = Collections.synchronizedList(new ArrayList<DrawObject>());
eqnLines = Collections.synchronizedList(new ArrayList<DrawObject>());
animPoints = Collections.synchronizedList(new ArrayList<DrawObject>());
animLines = Collections.synchronizedList(new ArrayList<DrawObject>());
animOvals = Collections.synchronizedList(new ArrayList<DrawObject>());
animRectangles = Collections.synchronizedList(new ArrayList<DrawObject>());
animLabels = Collections.synchronizedList(new ArrayList<DrawObject>());
df.setMaximumFractionDigits(4);
}
/** Ensures that the children are sorted in z-order sequence. */
private void ensureSorted() {
if (needsSorting) {
Collections.sort(children, FigureLayerComparator.INSTANCE);
needsSorting = false;
}
}
@Override
public java.util.List<Figure> getChildren() {
return Collections.unmodifiableList(children);
}
/**
* AbstractConnectionHandle factors the common code for handles that can be used to change the
* connection of a ConnectionFigure.
*
* <p>XXX - Undo/Redo is not implemented yet.
*
* @author Werner Randelshofer
* @version 3.0 2007-05-18 Changed due to changes in the canConnect methods of the ConnectionFigure
* interface. Shortened the name from AbstractChangeConnectionHandle to
* AbstractConnectionHandle. <br>
* 2.1 2006-02-16 Remove savedLiner from connection while tracking. <br>
* 2.0 2006-01-14 Changed to support double coordinates. <br>
* 1.0 2003-12-01 Derived from JHotDraw 5.4b1.
*/
public abstract class AbstractConnectionHandle extends AbstractHandle {
private Connector savedTarget;
private Connector connectableConnector;
private Figure connectableFigure;
private Point start;
/**
* We temporarily remove the Liner from the connection figure, while the handle is being moved. We
* store the Liner here, and add it back when the user has finished the interaction.
*/
private Liner savedLiner;
/** All connectors of the connectable Figure. */
protected Collection<Connector> connectors = Collections.emptyList();
/** Initializes the change connection handle. */
protected AbstractConnectionHandle(ConnectionFigure owner) {
super(owner);
}
public ConnectionFigure getOwner() {
return (ConnectionFigure) super.getOwner();
}
public boolean isCombinableWith(Handle handle) {
return false;
}
/** Returns the connector of the change. */
protected abstract Connector getTarget();
/** Disconnects the connection. */
protected abstract void disconnect();
/** Connect the connection with the given figure. */
protected abstract void connect(Connector c);
/** Sets the location of the connectableConnector point. */
protected abstract void setLocation(Point2D.Double p);
/** Returns the start point of the connection. */
protected abstract Point2D.Double getLocation();
/** Gets the side of the connection that is unaffected by the change. */
protected Connector getSource() {
if (getTarget() == getOwner().getStartConnector()) {
return getOwner().getEndConnector();
}
return getOwner().getStartConnector();
}
/** Disconnects the connection. */
public void trackStart(Point anchor, int modifiersEx) {
savedTarget = getTarget();
start = anchor;
savedLiner = getOwner().getLiner();
getOwner().setLiner(null);
// disconnect();
fireHandleRequestSecondaryHandles();
}
/** Finds a new connectableConnector of the connection. */
public void trackStep(Point anchor, Point lead, int modifiersEx) {
Point2D.Double p = view.viewToDrawing(lead);
view.getConstrainer().constrainPoint(p);
connectableFigure = findConnectableFigure(p, view.getDrawing());
if (connectableFigure != null) {
Connector aTarget = findConnectionTarget(p, view.getDrawing());
if (aTarget != null) {
p = aTarget.getAnchor();
}
}
getOwner().willChange();
setLocation(p);
getOwner().changed();
repaintConnectors();
}
/**
* Connects the figure to the new connectableConnector. If there is no new connectableConnector
* the connection reverts to its original one.
*/
public void trackEnd(Point anchor, Point lead, int modifiersEx) {
ConnectionFigure f = getOwner();
// Change node type
if ((modifiersEx
& (InputEvent.META_DOWN_MASK
| InputEvent.CTRL_DOWN_MASK
| InputEvent.ALT_DOWN_MASK
| InputEvent.SHIFT_DOWN_MASK))
!= 0
&& (modifiersEx & InputEvent.BUTTON2_DOWN_MASK) == 0) {
f.willChange();
int index = getBezierNodeIndex();
BezierPath.Node v = f.getNode(index);
if (index > 0 && index < f.getNodeCount()) {
v.mask = (v.mask + 3) % 4;
} else if (index == 0) {
v.mask = ((v.mask & BezierPath.C2_MASK) == 0) ? BezierPath.C2_MASK : 0;
} else {
v.mask = ((v.mask & BezierPath.C1_MASK) == 0) ? BezierPath.C1_MASK : 0;
}
f.setNode(index, v);
f.changed();
fireHandleRequestSecondaryHandles();
}
Point2D.Double p = view.viewToDrawing(lead);
view.getConstrainer().constrainPoint(p);
Connector target = findConnectionTarget(p, view.getDrawing());
if (target == null) {
target = savedTarget;
}
setLocation(p);
if (target != savedTarget) {
disconnect();
connect(target);
}
getOwner().setLiner(savedLiner);
getOwner().updateConnection();
connectableConnector = null;
connectors = Collections.emptyList();
}
private Connector findConnectionTarget(Point2D.Double p, Drawing drawing) {
Figure targetFigure = findConnectableFigure(p, drawing);
if (getSource() == null && targetFigure != null) {
return findConnector(p, targetFigure, getOwner());
} else if (targetFigure != null) {
Connector target = findConnector(p, targetFigure, getOwner());
if ((targetFigure != null)
&& targetFigure.canConnect()
&& targetFigure != savedTarget
&& !targetFigure.includes(getOwner())
&& (canConnect(getSource(), target)
|| getTarget() != null && getTarget().getOwner() == targetFigure)) {
return target;
}
}
return null;
}
protected abstract boolean canConnect(Connector existingEnd, Connector targetEnd);
protected Connector findConnector(Point2D.Double p, Figure f, ConnectionFigure prototype) {
return f.findConnector(p, prototype);
}
/** Draws this handle. */
public void draw(Graphics2D g) {
Graphics2D gg = (Graphics2D) g.create();
gg.transform(view.getDrawingToViewTransform());
for (Connector c : connectors) {
c.draw(gg);
}
gg.dispose();
drawCircle(g, (getTarget() == null) ? Color.red : Color.green, Color.black);
}
private Figure findConnectableFigure(Point2D.Double p, Drawing drawing) {
for (Figure f : drawing.getFiguresFrontToBack()) {
if (!f.includes(getOwner()) && f.canConnect() && f.contains(p)) {
return f;
}
}
return null;
}
protected void setPotentialTarget(Connector newTarget) {
this.connectableConnector = newTarget;
}
protected Rectangle basicGetBounds() {
// if (connection.getPointCount() == 0) return new Rectangle(0, 0, getHandlesize(),
// getHandlesize());
Point center = view.drawingToView(getLocation());
return new Rectangle(
center.x - getHandlesize() / 2,
center.y - getHandlesize() / 2,
getHandlesize(),
getHandlesize());
}
protected BezierFigure getBezierFigure() {
return (BezierFigure) getOwner();
}
protected abstract int getBezierNodeIndex();
@Override
public final Collection<Handle> createSecondaryHandles() {
LinkedList<Handle> list = new LinkedList<Handle>();
if (((ConnectionFigure) getOwner()).getLiner() == null && savedLiner == null) {
int index = getBezierNodeIndex();
BezierFigure f = getBezierFigure();
BezierPath.Node v = f.getNode(index);
if ((v.mask & BezierPath.C1_MASK) != 0 && (index != 0 || f.isClosed())) {
list.add(new BezierControlPointHandle(f, index, 1));
}
if ((v.mask & BezierPath.C2_MASK) != 0 && (index < f.getNodeCount() - 1 || f.isClosed())) {
list.add(new BezierControlPointHandle(f, index, 2));
}
if (index > 0 || f.isClosed()) {
int i = (index == 0) ? f.getNodeCount() - 1 : index - 1;
v = f.getNode(i);
if ((v.mask & BezierPath.C2_MASK) != 0) {
list.add(new BezierControlPointHandle(f, i, 2));
}
}
if (index < f.getNodeCount() - 2 || f.isClosed()) {
int i = (index == f.getNodeCount() - 1) ? 0 : index + 1;
v = f.getNode(i);
if ((v.mask & BezierPath.C1_MASK) != 0) {
list.add(new BezierControlPointHandle(f, i, 1));
}
}
}
return list;
}
protected BezierPath.Node getBezierNode() {
int index = getBezierNodeIndex();
return getBezierFigure().getNodeCount() > index ? getBezierFigure().getNode(index) : null;
}
public String getToolTipText(Point p) {
ConnectionFigure f = (ConnectionFigure) getOwner();
if (f.getLiner() == null && savedLiner == null) {
ResourceBundleUtil labels = ResourceBundleUtil.getLAFBundle("org.jhotdraw.draw.Labels");
BezierPath.Node node = getBezierNode();
return (node == null)
? null
: labels.getFormatted(
"bezierNodeHandle.tip",
labels.getFormatted(
(node.getMask() == 0)
? "bezierNode.linearNode"
: ((node.getMask() == BezierPath.C1C2_MASK)
? "bezierNode.cubicNode"
: "bezierNode.quadraticNode")));
} else {
return null;
}
}
/**
* Updates the list of connectors that we draw when the user moves or drags the mouse over a
* figure to which can connect.
*/
public void repaintConnectors() {
Rectangle2D.Double invalidArea = null;
for (Connector c : connectors) {
if (invalidArea == null) {
invalidArea = c.getDrawingArea();
} else {
invalidArea.add(c.getDrawingArea());
}
}
connectors =
(connectableFigure == null)
? new java.util.LinkedList<Connector>()
: connectableFigure.getConnectors(getOwner());
for (Connector c : connectors) {
if (invalidArea == null) {
invalidArea = c.getDrawingArea();
} else {
invalidArea.add(c.getDrawingArea());
}
}
if (invalidArea != null) {
view.getComponent().repaint(view.drawingToView(invalidArea));
}
}
}
// public Point2D.Double localize(double odo_x, double odo_y, double odo_theta, List<Fiducial>
// fiducials){
public Point2D.Double localize(double odo_x, double odo_y, List<Fiducial> fiducials) {
double x_disp = odo_x - prev_odo_x;
double y_disp = odo_y - prev_odo_y;
prev_odo_x = odo_x;
prev_odo_y = odo_y;
// double theta_disp = odo_theta - prev_odo_theta;
// prev_odo_theta = odo_theta;
List<Point2D.Double> new_particles = new ArrayList<Point2D.Double>();
List<Double> new_probs = new ArrayList<Double>();
List<Double> cumulative_probs = new ArrayList<Double>();
Point2D.Double curr_point;
double curr_prob, distance, mapx, mapy;
double probsum = 0;
for (int i = 0; i < NUM_PARTICLES; i++) {
// Motion Update
curr_point = new_particles.get(i);
new_particles.add(new Point2D.Double(curr_point.x + x_disp, curr_point.y + y_disp));
// Sensor Update
curr_prob = 1;
for (Fiducial fid : fiducials) {
mapx = fiducialPairs.get(fid.colors).x;
mapy = fiducialPairs.get(fid.colors).y;
distance =
Math.sqrt(
(curr_point.x + x_disp - mapx) * (curr_point.x + x_disp - mapx)
+ (curr_point.y + y_disp - mapy) * (curr_point.y + y_disp - mapy));
curr_prob *=
(Math.exp(-0.5 * (fid.distance - distance) * (fid.distance - distance) / SIGMA)
/ Math.sqrt(2 * Math.PI * SIGMA * SIGMA));
}
probsum += curr_prob;
new_probs.add(curr_prob);
cumulative_probs.add(probsum);
}
particles.clear();
double rand_val;
int index;
int max_index = 0;
for (int i = 0; i < NUM_PARTICLES; i++) {
rand_val = probsum * Math.random();
index = Math.abs(Collections.binarySearch(cumulative_probs, rand_val));
particles.add(new_particles.get(index));
if (new_probs.get(max_index) < new_probs.get(i)) {
max_index = i;
}
}
localPoint = new_particles.get(max_index);
return localPoint;
}
public OdoPoint localize(
double odo_x, double odo_y, double odo_theta, List<FiducialBlob> fiducials) {
double x_disp = odo_x - prev_odo_x;
double y_disp = odo_y - prev_odo_y;
double theta_disp = odo_theta - prev_odo_theta;
prev_odo_x = odo_x;
prev_odo_y = odo_y;
prev_odo_theta = odo_theta;
localPoint =
new OdoPoint(localPoint.x + x_disp, localPoint.y + y_disp, localPoint.theta + theta_disp);
List<FiducialBlob> revFiducials = new ArrayList<FiducialBlob>();
double mapx, mapy;
Color[] colors = new Color[2];
Fiducial mapFid;
double mapbearing;
double localAngle = localPoint.theta;
localAngle = localAngle - ((int) (localAngle / (2 * Math.PI)));
if (localAngle > Math.PI) {
localAngle -= 2 * Math.PI;
}
List<OdoPoint> new_particles = new ArrayList<OdoPoint>();
List<Double> new_probs = new ArrayList<Double>();
List<Double> cumulative_probs = new ArrayList<Double>();
OdoPoint curr_point;
double curr_prob, distance, fid_distance, fid_bearing;
double probsum = 0;
// Filter out unreasonable fiducials
for (FiducialBlob fid : fiducials) {
colors[0] = fid.colors[0];
colors[1] = fid.colors[1];
if (findFiducial(colors[0], colors[1]) != null) {
mapFid = findFiducial(colors[0], colors[1]);
mapx = mapFid.getPosition().x;
mapy = mapFid.getPosition().y;
mapbearing = Math.atan2(mapy - localPoint.y, mapx - localPoint.x);
distance =
Math.sqrt(
(localPoint.x - mapx) * (localPoint.x - mapx)
+ (localPoint.y - mapy) * (localPoint.y - mapy));
fid_distance = ImageBlob.computeDistance(fid.size, mapFid.getTopSize());
System.out.println(
"Localization: map and camera bearing = " + mapbearing + ", " + localAngle);
System.out.println(
"Localization: map and camera distance = " + distance + ", " + fid_distance);
if ((Math.abs(mapbearing - localAngle) < Math.PI / 6
|| Math.abs(mapbearing - localAngle + 2 * Math.PI) < Math.PI / 6
|| Math.abs(mapbearing - localAngle - 2 * Math.PI) < Math.PI / 6)
&& fid_distance < distance * 1.3
&& distance < fid_distance * 1.3) {
System.out.println("Localization: added fiducial");
revFiducials.add(fid);
} else {
System.out.println("Localization: eliminated fiducial from bearing and distance");
}
} else {
System.out.println("Localization: eliminated fiducial from colors");
}
}
/*// Use Odometry if no fiducials
if (revFiducials.size() == 0){
System.out.println("Localization: no fiducials to localize with");
particles.clear();
OdoPoint curr_point;
for (int i = 0; i < NUM_PARTICLES; i++){
curr_point = particles.get(i);
particles.add(new OdoPoint(curr_point.x + x_disp, curr_point.y + y_disp, curr_point.theta + theta_disp));
}
return localPoint;
}*/
for (int i = 0; i < NUM_PARTICLES; i++) {
// Motion Update
curr_point = particles.get(i);
new_particles.add(
new OdoPoint(
curr_point.x + x_disp, curr_point.y + y_disp, curr_point.theta + theta_disp));
// Sensor Update
curr_prob = 1;
for (FiducialBlob fid : revFiducials) {
colors[0] = fid.colors[0];
colors[1] = fid.colors[1];
mapFid = findFiducial(colors[0], colors[1]);
mapx = mapFid.getPosition().x;
mapy = mapFid.getPosition().y;
distance =
Math.sqrt(
(curr_point.x + x_disp - mapx) * (curr_point.x + x_disp - mapx)
+ (curr_point.y + y_disp - mapy) * (curr_point.y + y_disp - mapy));
fid_distance = ImageBlob.computeDistance(fid.size, mapFid.getTopSize());
curr_prob *=
NORMALIZATION
* (Math.exp(
-0.5 * (fid_distance - distance) * (fid_distance - distance) / DIST_SIGMA)
/ Math.sqrt(2 * Math.PI * DIST_SIGMA * DIST_SIGMA));
fid_bearing = curr_point.theta + theta_disp - ImageBlob.computeBearing(fid.x);
fid_bearing = fid_bearing - ((int) (fid_bearing / (2 * Math.PI)));
if (fid_bearing > Math.PI) {
fid_bearing -= 2 * Math.PI;
}
mapbearing = Math.atan2(mapy - curr_point.y - y_disp, mapx - curr_point.x - x_disp);
curr_prob *=
NORMALIZATION
* (Math.exp(
-0.5 * (mapbearing - fid_bearing) * (mapbearing - fid_bearing) / ROT_SIGMA)
/ Math.sqrt(2 * Math.PI * ROT_SIGMA * ROT_SIGMA));
}
probsum += curr_prob;
new_probs.add(curr_prob);
cumulative_probs.add(probsum);
}
if (revFiducials.size() > 0) {
System.out.println("Localization: localizing with fiducials");
particles.clear();
double rand_val;
int index;
int max_index = 0;
for (int i = 0; i < NUM_PARTICLES; i++) {
rand_val = probsum * Math.random();
index = Collections.binarySearch(cumulative_probs, rand_val);
if (index < 0) {
index = -(index + 1);
}
particles.add(new_particles.get(index));
if (new_probs.get(max_index) < new_probs.get(i)) {
max_index = i;
}
}
localPoint = new_particles.get(max_index);
} else {
System.out.println("Localization: no fiducials to localize with");
particles = new_particles;
}
return localPoint;
}
long finishBuild() {
Collections.sort(graph_objects, new HistoryCompare());
return last_time;
}