}
final Vector res;
this.path = path;
.createInitialLearnedObject();
final Vector loc = inferredEdge.getPointOnEdge(initialObservation.getObsPoint());
/*
* Sample the velocity
*/
belief.setMean(this.movementFilter.sampleStateBelief(belief.getMean(), rng));
belief.getMean().setElement(0,
inferredEdge.getStartPoint().euclideanDistance(loc));
assert Double.compare(Math.abs(this.belief.getMean().getElement(0)), inferredEdge.getLength()) <= 0;
}
this.edge = inferredEdge;
this.initialBelief = belief.clone();
this.path = InferredPath.getInferredPath(this.edge);
this.observation = initialObservation;
this.graph = graph;
this.edgeTransitionDist = new EdgeTransitionDistributions(
this.graph, parameters.getOnTransitionProbs(),
parameters.getOffTransitionProbs());
this.distanceFromPreviousState = 0d;
// DEBUG
// this.initialHashCode = this.hashCode();
// this.edgeInitialHashCode = this.edge.hashCode();
/*
}
// this.transInitialHashCode = this.edgeTransitionDist.hashCode();
// this.beliefInitialHashCode =
// Arrays.hashCode(((DenseVector)this.initialBelief.convertToVector()).getArray());
// this.obsInitialHashCode = this.observation.hashCode();
}
public VehicleState(OtpGraph graph, Observation observation,
StandardRoadTrackingFilter filter, MultivariateGaussian belief,
EdgeTransitionDistributions edgeTransitionDist,
InferredPath path, VehicleState state) {
Preconditions
.checkArgument(!(belief.getInputDimensionality() == 2 && path.isEmptyPath()));
Preconditions.checkNotNull(state);
Preconditions.checkNotNull(graph);
Preconditions.checkNotNull(observation);
Preconditions.checkNotNull(filter);
Preconditions.checkNotNull(belief);
this.observation = observation;
this.movementFilter = filter;
this.belief = belief.clone();
this.graph = graph;
* This is the constructor used when creating transition states, so this is
* where we'll need to reset the distance measures
*/
if (this.belief.getInputDimensionality() == 2) {
/*
* IMPORTANT: the filtering that occurs on edges doesn't mean
* that the filtered location will end up on the same edge.
*/
final double distPosition = this.belief.getMean().getElement(0);
PathEdge pathEdge = path.getEdgeForDistance(distPosition, true);
if (pathEdge == null) {
/*
* Sometimes the distance-based position is outside
* of the path's bounds. Determine which side it's closest
* to.
*/
if (path.isBackward()) {
if (distPosition < path.getTotalPathDistance()) {
this.belief.getMean().setElement(0, path.getTotalPathDistance());
pathEdge = Iterables.getLast(path.getEdges());
} else {
this.belief.getMean().setElement(0, 0d);
pathEdge = Iterables.getFirst(path.getEdges(), null);
}
} else {
if (distPosition > path.getTotalPathDistance()) {
this.belief.getMean().setElement(0, path.getTotalPathDistance());
pathEdge = Iterables.getLast(path.getEdges());
} else {
this.belief.getMean().setElement(0, 0d);
pathEdge = Iterables.getFirst(path.getEdges(), null);
}
}
}
this.edge = pathEdge.getInferredEdge();
this.distanceFromPreviousState = pathEdge.getDistToStartOfEdge();
/*
* We normalized the position relative to the direction of motion.
*/
final double normalizedEdgeLoc = this.belief.getMean().getElement(0)
- pathEdge.getDistToStartOfEdge();
final double desiredDirection = Math.signum(this.belief.getMean().getElement(1));
if (desiredDirection == 0d
|| Math.signum(normalizedEdgeLoc) == desiredDirection) {
this.belief.getMean().setElement(
0, normalizedEdgeLoc);
} else {
this.belief.getMean().setElement(
0, desiredDirection * edge.getLength() +
normalizedEdgeLoc);
}
assert Double.compare(Math.abs(this.belief.getMean().getElement(0)), edge.getLength()) <= 0;
} else {
this.edge = InferredEdge.getEmptyEdge();
this.distanceFromPreviousState = null;
}
/*
* This has to come after the adjustment
*/
this.initialBelief = this.belief.clone();
this.edgeTransitionDist = edgeTransitionDist;
this.parentState = state;
/*
* Reset the parent's parent state so that we don't keep these objects
* forever.
*/
// state.parentState = null;
final double timeDiff;
if (observation.getPreviousObservation() != null) {
timeDiff = (observation.getTimestamp().getTime() - observation
.getPreviousObservation().getTimestamp().getTime()) / 1000d;
} else {
timeDiff = 30d;
}
this.movementFilter.setCurrentTimeDiff(timeDiff);
// DEBUG
// this.initialHashCode = this.hashCode();
// this.edgeInitialHashCode = this.edge.hashCode();
// this.transInitialHashCode = this.edgeTransitionDist.hashCode();
// this.beliefInitialHashCode =
// Arrays.hashCode(((DenseVector)this.initialBelief.convertToVector()).getArray());
// this.obsInitialHashCode = this.observation.hashCode();
}
public VehicleState(VehicleState other) {
this.graph = other.graph;
this.movementFilter = other.movementFilter.clone();
this.belief = other.belief.clone();
this.edgeTransitionDist = other.edgeTransitionDist.clone();
// TODO clone this?
this.edge = other.edge;
this.observation = other.observation;
this.distanceFromPreviousState = other.distanceFromPreviousState;
this.parentState = other.parentState;
this.initialBelief = other.initialBelief.clone();
this.path = other.path;
// DEBUG
// this.initialHashCode = this.hashCode();
// this.edgeInitialHashCode = this.edge.hashCode();
// this.transInitialHashCode = this.edgeTransitionDist.hashCode();
// this.beliefInitialHashCode =
// Arrays.hashCode(((DenseVector)this.initialBelief.convertToVector()).getArray());
// this.obsInitialHashCode = this.observation.hashCode();
}
@Override
public VehicleState clone() {
return new VehicleState(this);
}
@Override
public int compareTo(VehicleState arg0) {
return oneStateCompareTo(this, arg0);
}
@Override
public boolean equals(Object obj) {
if (this == obj) {
return true;
}
if (obj == null) {
return false;
}
if (getClass() != obj.getClass()) {
return false;
}
final VehicleState other = (VehicleState) obj;
final DenseVector thisV = (DenseVector) initialBelief
.convertToVector();
final DenseVector otherV = (DenseVector) other.initialBelief
.convertToVector();
if (initialBelief == null) {
if (other.initialBelief != null) {
return false;
}
} else if (!Arrays.equals(thisV.getArray(), otherV.getArray())) {
return false;
}
if (edge == null) {
if (other.edge != null) {
return false;
}
} else if (!edge.equals(other.edge)) {
return false;
}
if (edgeTransitionDist == null) {
if (other.edgeTransitionDist != null) {
return false;
}
} else if (!edgeTransitionDist.equals(other.edgeTransitionDist)) {
return false;
}
if (observation == null) {
if (other.observation != null) {
return false;
}
} else if (!observation.equals(other.observation)) {
return false;
}
return true;
}
public MultivariateGaussian getBelief() {
return belief;
public Double getDistanceFromPreviousState() {
return distanceFromPreviousState;
}
public InferredEdge getEdge() {
return edge;
}
public EdgeTransitionDistributions getEdgeTransitionDist() {
return edgeTransitionDist;
}
public OtpGraph getGraph() {
return graph;
}
/**
* Gets the belief in ground coordinates (even if it's really tracking road
* coordinates).
*
* @return
*/
public MultivariateGaussian getGroundOnlyBelief() {
if (belief.getInputDimensionality() == 2) {
final MultivariateGaussian beliefProj = new MultivariateGaussian();
StandardRoadTrackingFilter.convertToGroundBelief(
beliefProj, PathEdge.getEdge(this.edge, 0d));
return beliefProj;
} else {
return belief;
}
public InferredEdge getInferredEdge() {
return this.edge;
}
public MultivariateGaussian getInitialBelief() {
return initialBelief;
}
public KalmanFilter getKalmanFilter() {
return this.belief.getInputDimensionality() == 4 ? this
.getMovementFilter().getGroundFilter() : this
.getMovementFilter().getRoadFilter();
}
/**
* Returns ground-coordinate mean location
*
* @return
*/
public Vector getMeanLocation() {
Vector v;
if (belief.getInputDimensionality() == 2) {
Preconditions.checkArgument(!this.edge.isEmptyEdge());
final MultivariateGaussian projBelief = belief.clone();
StandardRoadTrackingFilter.convertToGroundBelief(
projBelief, PathEdge.getEdge(this.edge, 0d));
v = projBelief.getMean();
} else {
v = belief.getMean();
}
return VectorFactory.getDefault().createVector2D(
v.getElement(0), v.getElement(2));
}
public StandardRoadTrackingFilter getMovementFilter() {
return movementFilter;
}
public Vector getNonVelocityVector() {
return getNonVelocityVector(this.belief.getMean());
}
public Observation getObservation() {
return observation;
}
public VehicleState getParentState() {
return parentState;
}
public InferredPath getPath() {
return path;
}
@Override
public VehicleState.PDF getProbabilityFunction() {
return new VehicleState.PDF(this);
}
@Override
public int hashCode() {
final int prime = 31;
int result = 1;
final DenseVector thisV = (DenseVector) initialBelief
.convertToVector();
result = prime
* result
+ ((initialBelief == null) ? 0 : Arrays.hashCode(thisV
.getArray()));
result = prime * result + ((edge == null) ? 0 : edge.hashCode());
result = prime
* result
+ ((edgeTransitionDist == null) ? 0 : edgeTransitionDist
.hashCode());
result = prime * result
+ ((observation == null) ? 0 : observation.hashCode());
return result;
}
@Override
public VehicleStateConditionalParams sample(Random random) {
throw new NotImplementedError();
}
@Override
public ArrayList sample(
Random random, int numSamples) {
throw new NotImplementedError();
}
public void setParentState(VehicleState parentState) {
this.parentState = parentState;
}
@Override
public String toString() {
return Objects.toStringHelper(this).add("belief", belief)
.add("edge", edge.getEdgeId())
.addValue(observation.getTimestamp()).toString();
}
public static double getDvariance() {
return dVariance;
}
public static double getGvariance() {
return gVariance;
}
public static Vector getNonVelocityVector(Vector vector) {
if (vector.getDimensionality() == 4)
res = StandardRoadTrackingFilter.getOg().times(vector);
else
res = StandardRoadTrackingFilter.getOr().times(vector);
return res;
}
public static long getSerialversionuid() {
return serialVersionUID;
}
public static double getVvariance() {
return vVariance;
}
private static int oneStateCompareTo(VehicleState t, VehicleState o) {
if (t == o)
return 0;
if (t == null) {
if (o != null)
return -1;
} else if (o == null) {
return 1;
}
final DenseVector thisV = (DenseVector) t.initialBelief
.convertToVector();
final DenseVector otherV = (DenseVector) o.initialBelief
.convertToVector();
final CompareToBuilder comparator = new CompareToBuilder();
comparator.append(thisV.getArray(), otherV.getArray());
comparator.append(t.getObservation(), o.getObservation());
comparator.append(t.edge, o.edge);
comparator.append(t.edgeTransitionDist, o.edgeTransitionDist);
return comparator.toComparison();
}
} |