public double getChi2(Graph g) {
double chi2 = Double.MAX_VALUE;
for (int i = 0; i < components.length; i++) {
chi2 = Math.min(chi2, -2 * Math.log(weights[i]) + components[i].getChi2(g));
}
return chi2;
}
// incorporates exponential model of probability of motion, weight less likely motions less
private double[] constructW(double[] Uxyzrpy) {
assert (Uxyzrpy.length == Vxyzrpy.length) : "Warning expected 6 velocity";
double[] W = new double[6]; // weighting matrix
for (int i = 0; i < Uxyzrpy.length; i++) {
double u = Uxyzrpy[i];
double v = Vxyzrpy[i];
double Dv = Math.abs(u - v);
if (i <= 2) {
// handle velocities close to 0
if ((Math.abs(u) <= LV_THRESH) && (Math.abs(v) <= LV_THRESH)) {
// use exponential model to come up with weights
W[i] = MathUtil.exp(-ALPHA * Dv);
} // if our change in velocity was within acceptable bounds
else if ((Dv / u) <= DELTAV_THRESH) {
// use exponential model to come up with weights
W[i] = MathUtil.exp(-ALPHA * Dv);
} else {
W[i] = 0; // throw out bad changes
}
} else {
// handle angular velocities close to 0
if ((Math.abs(u) <= LR_THRESH) && (Math.abs(v) <= LR_THRESH)) {
// use exponential model to come up with weights
W[i] = MathUtil.exp(-BETA * Dv);
} // if our change in velocity was within acceptable bounds
else if ((Dv / u) <= DELTAR_THRESH) {
// use exponential model to come up with weights
W[i] = MathUtil.exp(-BETA * Dv);
} else {
W[i] = 0; // throw out bad changes
}
}
}
return W;
}
public Linearization linearize(Graph g, Linearization lin) {
double bestChi2 = Double.MAX_VALUE;
GEdge bestEdge = null;
int besti = -1;
for (int i = 0; i < components.length; i++) {
double thisChi2 = -2 * Math.log(weights[i]) + components[i].getChi2(g);
if (thisChi2 < bestChi2) {
bestChi2 = thisChi2;
bestEdge = components[i];
besti = i;
}
System.out.printf("%d %15f\n", i, thisChi2);
}
System.out.println("besti: " + besti);
return bestEdge.linearize(g, lin);
}
