public BoxShape transform(double T[][]) {
BoxShape bs = new BoxShape();
bs.sxyz = LinAlg.copy(sxyz);
bs.vertices = LinAlg.transform(T, vertices);
bs.planes = LinAlg.transformPlanes(T, planes);
return bs;
}
// if we don't have anything to filter, just use Dead Reckoning
public double[][] estimate() {
// use new velocity estimates to estimate xyzrpy
double[] Exyzrpy = LinAlg.scale(Vxyzrpy, T);
// convert back to RBT
double[][] Erbt = LinAlg.xyzrpyToMatrix(Exyzrpy);
// XXX there is a 2nd version of the function above which one is better
return Erbt;
}
/**
* Gets the center point for the reflection
*
* @param worst the index of the worst point
* @return the center point for the reflection, such that <CODE>
* x = 1 / dimX(x) * sum(x(k,i), i = 0..dimX(x), i != worst)</CODE>
*/
protected double[] getXCenter(int worst) {
double[] xCenter = new double[dimX];
LinAlg.initialize(xCenter, 0);
for (int i = 0; i < dimXP1; i++) {
if (i != worst) xCenter = LinAlg.add(xCenter, x[i].getX());
}
return LinAlg.multiply(1 / ((double) dimX), xCenter);
}
/**
* Determines whether a restart with a smaller simplex should be tried or not
*
* @return <CODE>true</CODE> if restart can be tried, <CODE>false</CODE> otherwise
*/
protected boolean restartCriterion() {
double f, S1, S2;
double[] fX = new double[dimXP1];
for (int i = 0; i < dimXP1; i++) fX[i] = x[i].getF(0);
S1 = LinAlg.twoNorm(fX);
S1 *= S1;
S2 = LinAlg.oneNorm(fX);
S2 *= S2;
S2 /= (double) (dimXP1);
f = (S1 - S2) / (double) dimX;
if (f < (sqEps)) return true;
else return false;
}
public static void mixture() {
Graph g = new Graph();
int cnt = 5;
for (int i = 0; i < cnt; i++) {
GXYTNode gn = new GXYTNode();
gn.init = new double[] {i, 0, 0};
gn.state = LinAlg.copy(gn.init);
gn.truth = new double[3];
g.nodes.add(gn);
if (i > 0) {
double sig = 0.06;
// wheel slippage mode
GXYTEdge ge1 = new GXYTEdge();
ge1.z = new double[] {0, 0, 0};
ge1.P = LinAlg.diag(new double[] {sig, sig, sig});
ge1.nodes = new int[] {i - 1, i};
// nominal odometry
GXYTEdge ge2 = new GXYTEdge();
ge2.z = new double[] {1, 0, 0};
ge2.P = LinAlg.diag(new double[] {sig, sig, sig});
ge2.nodes = new int[] {i - 1, i};
GEdgeMixture ge = new GEdgeMixture(new GEdge[] {ge1, ge2}, new double[] {0.05, 0.95});
g.edges.add(ge);
}
}
if (true) {
GXYTEdge ge = new GXYTEdge();
ge.z = new double[] {cnt, 0, 0};
ge.P = LinAlg.diag(new double[] {0.1, 0.1, 0.1});
ge.nodes = new int[] {0, g.nodes.size() - 1};
g.edges.add(ge);
}
try {
g.write("mixture.graph");
} catch (IOException ex) {
System.out.println("ex: " + ex);
}
}
public void addLayer(VisLayer layer, double fviewport[]) {
FPosition fpos = new FPosition();
fpos.layer = layer;
fpos.fviewport = LinAlg.copy(fviewport);
fposes.add(fpos);
vc.draw();
}
// weight A by alpha and B by 1-alpha if alpha == 1 then returns A
private double[] weightedSum(double[] A, double[] B, double[] W) {
assert ((A.length == B.length) && (A.length == W.length)) : "Warning!";
double[] C = new double[A.length];
for (int i = 0; i < A.length; i++) {
C[i] = (1 - W[i]) * A[i] + W[i] * B[i];
if (C[i] == Double.NaN) {
System.out.println("Warning generated NaN: T = " + T);
LinAlg.print(W);
}
}
return C;
}
// incorporate new information and return new estimate
public double[][] estimate(double[][] Urbt) {
// what rigid body transformation occured over the last frame
double[] DUxyzrpy = LinAlg.matrixToXyzrpy(Urbt);
// convert to velocity
double[] Uxyzrpy;
if (T != 0) {
Uxyzrpy = LinAlg.scale(DUxyzrpy, 1 / T);
} else {
Uxyzrpy = new double[6];
}
/*
System.out.println("Our observation velocity");
LinAlg.print(Uxyzrpy);
System.out.println("Our best estimate of velocity");
LinAlg.print(Vxyzrpy);
*/
// calculate relative weighting
double[] W = constructW(Uxyzrpy);
/*
System.out.println("Our weighting matrix");
LinAlg.print(W);
*/
// incorporatenew information
Vxyzrpy = weightedSum(Vxyzrpy, Uxyzrpy, W);
/*
System.out.println("After in corporating information");
LinAlg.print(Vxyzrpy);
*/
// use new velocity estimates to estimate xyzrpy
double[] Exyzrpy = LinAlg.scale(Vxyzrpy, T);
// convert back to RBT
double[][] Erbt = LinAlg.xyzrpyToMatrix(Exyzrpy);
// XXX there is a 2nd version of the function above which one is better
return Erbt;
}
public static void nomixture() {
Graph g = new Graph();
int cnt = 5;
double u = 0.95;
double sig = 0.1075;
for (int i = 0; i < cnt; i++) {
GXYTNode gn = new GXYTNode();
gn.init = new double[] {i, 0, 0};
gn.state = LinAlg.copy(gn.init);
g.nodes.add(gn);
if (i > 0) {
GXYTEdge ge = new GXYTEdge();
ge.z = new double[] {0.95, 0, 0};
ge.P = LinAlg.diag(new double[] {sig, sig, sig});
ge.nodes = new int[] {i - 1, i};
g.edges.add(ge);
}
}
if (true) {
GXYTEdge ge = new GXYTEdge();
ge.z = new double[] {0, 0, 0};
ge.P = LinAlg.diag(new double[] {0.1, 0.1, 0.1});
ge.nodes = new int[] {0, g.nodes.size() - 1};
g.edges.add(ge);
}
try {
g.write("nomixture.graph");
} catch (IOException ex) {
System.out.println("ex: " + ex);
}
}
public void read(StructureReader ins) throws IOException {
int ncomponents = ins.readInt();
components = new GEdge[ncomponents];
weights = new double[ncomponents];
for (int i = 0; i < components.length; i++) {
weights[i] = ins.readDouble();
String cls = ins.readString();
components[i] = (GEdge) ReflectUtil.createObject(cls);
ins.blockBegin();
components[i].read(ins);
ins.blockEnd();
}
attributes = Attributes.read(ins);
nodes = LinAlg.copy(components[0].nodes);
}
/** Components should all be of the same edge type. Weights should sum to 1. * */
public GEdgeMixture(GEdge components[], double weights[]) {
this.components = components;
this.weights = weights;
double total = 0;
assert (components.length == weights.length);
for (int i = 0; i < components.length; i++) {
total += weights[i];
assert (components[i].getClass().equals(components[0].getClass()));
assert (components[i].nodes.length == components[0].nodes.length);
for (int j = 0; j < components[0].nodes.length; j++) {
assert (components[i].nodes[j] == components[0].nodes[j]);
}
}
assert (total >= 0.99 && total <= 1.01);
this.nodes = LinAlg.copy(components[0].nodes);
}
public BoxShape(double sxyz[]) {
this.sxyz = LinAlg.copy(sxyz);
planes = new ArrayList<double[]>();
vertices = new ArrayList<double[]>();
// create planes and vertices
planes.add(new double[] {-1, 0, 0, -sxyz[0] / 2});
planes.add(new double[] {1, 0, 0, -sxyz[0] / 2});
planes.add(new double[] {0, -1, 0, -sxyz[1] / 2});
planes.add(new double[] {0, 1, 0, -sxyz[1] / 2});
planes.add(new double[] {0, 0, -1, -sxyz[2] / 2});
planes.add(new double[] {0, 0, 1, -sxyz[2] / 2});
for (int i = -1; i < 2; i += 2)
for (int j = -1; j < 2; j += 2)
for (int k = -1; k < 2; k += 2)
vertices.add(new double[] {i * sxyz[0] / 2, j * sxyz[1] / 2, k * sxyz[2] / 2});
}
public VzOFF(String path, VzMesh.Style style) throws IOException {
this.path = path;
this.style = style;
BufferedReader ins = new BufferedReader(new FileReader(new File(path)));
FloatArray vertexArray = new FloatArray();
IntArray indexArray = new IntArray();
FloatArray normalArray;
String header = ins.readLine();
if (!header.equals("OFF")) throw new IOException("Not an OFF file");
int nvertexArray, nfaces, nedges;
if (true) {
String sizes = ins.readLine();
String toks[] = fastSplit(sizes);
nvertexArray = Integer.parseInt(toks[0]);
nfaces = Integer.parseInt(toks[1]);
nedges = Integer.parseInt(toks[2]);
}
for (int i = 0; i < nvertexArray; i++) {
String line = ins.readLine();
String toks[] = fastSplit(line);
float x = Float.parseFloat(toks[0]);
float y = Float.parseFloat(toks[1]);
float z = Float.parseFloat(toks[2]);
xyz_min[0] = Math.min(xyz_min[0], x);
xyz_min[1] = Math.min(xyz_min[1], y);
xyz_min[2] = Math.min(xyz_min[2], z);
xyz_max[0] = Math.max(xyz_max[0], x);
xyz_max[1] = Math.max(xyz_max[1], y);
xyz_max[2] = Math.max(xyz_max[2], z);
vertexArray.add(x);
vertexArray.add(y);
vertexArray.add(z);
}
float vs[] = vertexArray.getData();
float ns[] = new float[vs.length];
normalArray = new FloatArray(ns);
for (int i = 0; i < nfaces; i++) {
String line = ins.readLine();
String toks[] = fastSplit(line);
int len = Integer.parseInt(toks[0]);
assert (len + 1 == toks.length);
for (int j = 2; j + 1 <= len; j++) {
int a = Integer.parseInt(toks[1]);
int b = Integer.parseInt(toks[j]);
int c = Integer.parseInt(toks[j + 1]);
indexArray.add(a);
indexArray.add(b);
indexArray.add(c);
float vba[] =
new float[] {
vs[b * 3 + 0] - vs[a * 3 + 0],
vs[b * 3 + 1] - vs[a * 3 + 1],
vs[b * 3 + 2] - vs[a * 3 + 2]
};
float vca[] =
new float[] {
vs[c * 3 + 0] - vs[a * 3 + 0],
vs[c * 3 + 1] - vs[a * 3 + 1],
vs[c * 3 + 2] - vs[a * 3 + 2]
};
float n[] = LinAlg.normalize(LinAlg.crossProduct(vba, vca));
for (int k = 0; k < 3; k++) {
ns[3 * a + k] += n[k];
ns[3 * b + k] += n[k];
ns[3 * c + k] += n[k];
}
}
}
for (int i = 0; i + 2 < ns.length; i += 3) {
double mag = Math.sqrt(ns[i + 0] * ns[i + 0] + ns[i + 1] * ns[i + 1] + ns[i + 2] * ns[i + 2]);
ns[i + 0] /= mag;
ns[i + 1] /= mag;
ns[i + 2] /= mag;
}
mesh =
new VzMesh(
new VisVertexData(vs, vs.length / 3, 3),
new VisVertexData(ns, ns.length / 3, 3),
new VisIndexData(indexArray),
VzMesh.TRIANGLES);
ins.close();
}
/** Call with one or more OFF model paths * */
public static void main(String args[]) {
JFrame f = new JFrame("VzOFF " + args[0]);
f.setLayout(new BorderLayout());
VisWorld vw = new VisWorld();
VisLayer vl = new VisLayer(vw);
VisCanvas vc = new VisCanvas(vl);
VzMesh.Style defaultMeshStyle = new VzMesh.Style(Color.cyan);
ArrayList<VzOFF> models = new ArrayList<VzOFF>();
for (int i = 0; i < args.length; i++) {
if (args[i].endsWith(".off")) {
try {
models.add(new VzOFF(args[i], defaultMeshStyle));
System.out.printf("Loaded: %20s (%5.2f%%)\n", args[i], i * 100.0 / args.length);
} catch (IOException ex) {
System.out.println("ex: " + ex);
}
} else {
System.out.printf("Ignoring file with wrong suffix: " + args[i]);
}
}
if (models.size() == 0) {
System.out.println("No models specified\n");
return;
}
int rows = (int) Math.sqrt(models.size());
int cols = models.size() / rows + 1;
// VzGrid.addGrid(vw);
VisWorld.Buffer vb = vw.getBuffer("models");
for (int y = 0; y < rows; y++) {
for (int x = 0; x < cols; x++) {
int idx = y * cols + x;
if (idx >= models.size()) break;
VzOFF model = models.get(idx);
double mx =
Math.max(
model.xyz_max[2] - model.xyz_min[2],
Math.max(model.xyz_max[1] - model.xyz_min[1], model.xyz_max[0] - model.xyz_min[0]));
vb.addBack(
new VisChain(
LinAlg.translate(x + .5, rows - (y + .5), 0),
new VzRectangle(1, 1, new VzLines.Style(Color.white, 3)),
new VisChain(
LinAlg.translate(0, .4, 0),
new VzText(
VzText.ANCHOR.CENTER,
String.format("<<sansserif-20,scale=.003>>%s", baseName(model.path)))),
LinAlg.scale(.5, .5, .5),
LinAlg.scale(1.0 / mx, 1.0 / mx, 1.0 / mx),
LinAlg.translate(
-(model.xyz_max[0] + model.xyz_min[0]) / 2.0,
-(model.xyz_max[1] + model.xyz_min[1]) / 2.0,
-(model.xyz_max[2] + model.xyz_min[2]) / 2.0),
model));
}
}
vb.swap();
vl.cameraManager.fit2D(new double[] {0, 0, 0}, new double[] {cols, rows, 0}, true);
((DefaultCameraManager) vl.cameraManager).interfaceMode = 3.0;
f.add(vc);
f.setSize(600, 400);
f.setVisible(true);
}
/**
* Reflects a point
*
* @param xCenter the center point for normal reflection
* @param xWorst the point that has to be reflected
* @return the new point, such that <CODE>xNew = 2 * xCenter - xWorst</CODE>
*/
protected double[] reflect(double[] xCenter, double[] xWorst) {
return LinAlg.subtract(LinAlg.multiply(2, xCenter), xWorst);
}
/**
* Runs the optimization process until a termination criteria is satisfied
*
* @param x0 initial point
* @return <CODE>-1</CODE> if the maximum number of iteration is exceeded <br>
* <CODE>+1</CODE> if the required accuracy is reached
* @exception Exception
* @exception OptimizerException
*/
public int run(Point x0) throws OptimizerException, Exception {
boolean restart = false;
int retFla = 0;
int i, j, nue;
int w = 0;
int b = 0;
double ssf = 1;
int dimF = getDimensionF();
Point[] xkP1 = new Point[dimXP1]; // xkP1 = x(k+1)
for (i = 0; i < dimXP1; i++) xkP1[i] = new Point(dimX, 0, dimF);
Point xN = new Point(dimX, 0, dimF);
Point xNN = new Point(dimX, 0, dimF);
double[] xC = new double[dimX];
double[] xTemp = new double[dimX];
String[] com = new String[11];
com[1] = "Establish initial simplex (1).";
com[2] = "Reflection (2).";
com[4] = "Expansion (4).";
com[5] = "Partial outside contraction (5).";
com[6] = "Partial inside contraction (6).";
com[7] = "Total contraction to best known point (7).";
com[10] = "Invoke simplex reconstruction (10).";
final String comBesNowPoi = "Best known point in total contraction (7).";
final String comNewSte = "New step.";
final String comBasPoi = "Base point for optimality check.";
double[] dNew = new double[dimX]; // new movement direction of changed point
double[] dOld = new double[dimX]; // old movement direction of changed point
double dNewL2; // L2norm of dNew
int stepOld = 0; // number of last step section
double cosMov = -1; // inner product of the last two directions
boolean insideContraction = false; // flag whether we have just a inside contraction
// beyond us
int step = 0; // number of step
boolean tryRes;
boolean iterate = true;
int nexAllRes = konvge; // number of main iteration when next restart might
// be enabled
// Perturber for check of the optimaltity condition
Perturber per = new Perturber(this);
LinAlg.initialize(xC, 0);
// optimization loop
do {
switch (step) {
case 0: // Initalization
for (i = 0; i < dimX; i++) // starting point
x[0].setX(i, getX0(i));
step = 1;
break;
case 1: // Etablish the initial simplex
for (i = 1; i < dimXP1; i++) {
for (j = 0;
j < dimX;
j++) { // getX is used on x[0] since x[0] may be reassigned after a failed
// optimality check
if (i == j + 1) x[i].setX(j, x[0].getX(j) + ssf * getDx(j, x[0].getX(j)));
else x[i].setX(j, x[0].getX(j));
}
}
// get the objective function values for the initial simplex
if (restart) {
i = 1; // first point is already known from step 10
restart = false;
} else i = 0;
for (; i < dimXP1; i++) {
x[i].setComment(com[1]);
x[i] = getF(x[i]);
report(x[i], SUBITERATION);
if (i == 0) report(x[i], MAINITERATION);
checkObjectiveFunctionValue(x[i]);
}
step = 2;
break;
case 2: // determine worst and best point for the normal reflection
w = getWorst();
b = getBest();
xC = getXCenter(w);
xN.setComment(com[2]);
xN.setX(reflect(xC, x[w].getX()));
xN = getF(xN);
report(xN, SUBITERATION);
stepOld = step;
step = (xN.getF(0) < x[b].getF(0)) ? 4 : 3;
break;
case 3: // compare trial with other vertices
nue = dimXP1 - rank(xN.getF(0)); // if nue is 0, we got the worst point
if (nue == 0) // we got even a worser point than we had
step = 6;
else if (nue == 1) // we got a worse point but not that worse as the last was
step = 5;
else // we got a good point
{
xkP1[w] = (Point) xN.clone();
step = 8;
}
break;
case 4: // Expansion (try if further expansion of point is successfull)
xNN.setComment(com[4]);
xNN.setX(LinAlg.subtract(LinAlg.multiply(2., xN.getX()), xC));
xNN = getF(xNN);
report(xNN, SUBITERATION);
if (xNN.getF(0) < x[b].getF(0)) xkP1[w] = (Point) xNN.clone(); // expansion was successful
else xkP1[w] = (Point) xN.clone();
stepOld = step;
step = 8;
break;
case 5: // Partial outside contraction
xNN.setComment(com[5]);
xNN.setX(LinAlg.multiply(0.5, LinAlg.add(xC, xN.getX())));
xNN = getF(xNN);
report(xNN, SUBITERATION);
if (xNN.getF(0) <= xN.getF(0)) {
xkP1[w] = (Point) xNN.clone();
stepOld = step;
step = 8;
} else step = 7;
break;
case 6: // Partial inside contraction
xNN.setComment(com[6]);
xNN.setX(LinAlg.multiply(0.5, LinAlg.add(xC, x[w].getX())));
xNN = getF(xNN);
report(xNN, SUBITERATION);
/* the inequality (fNN >= fX[w]) is changed so that we
get a total contraction
if some vertices are in a null space.
Nelder Mead have in their paper (1965) the strict
inequality (fNN > fX[w]) 02/16/99 wm
*/
if (xNN.getF(0) >= x[w].getF(0)) step = 7;
else {
xkP1[w] = (Point) xNN.clone();
stepOld = step;
step = 8;
insideContraction = true;
}
break;
case 7: // Total contraction to best point
// construct new simplex
xkP1[b] = (Point) x[b].clone();
xkP1[b].setComment(comBesNowPoi);
report(xkP1[b], SUBITERATION);
report(xkP1[b], MAINITERATION);
xTemp = x[b].getX();
for (i = 0; i < dimXP1; i++) {
if (i != b) {
xkP1[i].setComment(com[7]);
xkP1[i].setX(LinAlg.multiply(0.5, LinAlg.add(xTemp, x[i].getX())));
xkP1[i] = getF(xkP1[i]);
report(xkP1[i], SUBITERATION);
report(xkP1[i], MAINITERATION);
checkObjectiveFunctionValue(xkP1[i]);
}
}
insideContraction = true;
step = 9;
break;
case 8: // normal iteration loop
for (i = 0; i < dimXP1; i++) {
if (i != w) xkP1[i] = (Point) x[i].clone();
else {
xkP1[w].setComment(com[stepOld]);
report(xkP1[w], MAINITERATION);
checkObjectiveFunctionValue(xkP1[w]);
}
}
step = 9;
break;
case 9: // Termination criterion
// increase k <- k+1
// Note: 9 is only entered from 7 or 8
// determine movement direction of simplex
if (modStoCri) {
System.arraycopy(dNew, 0, dOld, 0, dimX);
// new (current) search direction
double[][] temp1 = new double[dimXP1][dimX];
double[][] temp2 = new double[dimXP1][dimX];
for (i = 0; i < dimXP1; i++) {
temp1[i] = xkP1[i].getX();
temp2[i] = x[i].getX();
}
dNew = LinAlg.subtract(LinAlg.getCenter(temp1), LinAlg.getCenter(temp2));
dNewL2 = LinAlg.twoNorm(dNew);
if (dNewL2 > Double.MIN_VALUE) dNew = LinAlg.multiply(1 / dNewL2, dNew);
cosMov = LinAlg.innerProduct(dOld, dNew);
}
///////////////////////////////////////////////////////////
// update points
for (i = 0; i < dimXP1; i++) x[i] = (Point) xkP1[i].clone();
if (checkMaxIteration()) {
retFla = -1;
iterate = false;
break;
}
// check for restart
if (modStoCri)
tryRes =
(insideContraction
&& (cosMov <= 0)
&& (nexAllRes <= getMainIterationNumber())
&& restartCriterion());
else tryRes = (nexAllRes <= getMainIterationNumber()) && restartCriterion();
if (tryRes) {
// try a restart
step = 10;
nexAllRes = getMainIterationNumber() + konvge + 1;
} else {
// do a usual iteration loop
if (writeStepNumber()) {
// get the best point
b = getBest();
x[b].setComment(comNewSte);
x[b] = increaseStepNumber(x[b]);
report(x[b], MAINITERATION);
report(x[b], SUBITERATION);
}
step = 2;
}
// reset contraction flag
insideContraction = false;
break;
case 10: // restart test (note that w points to the new vertex)
ssf = cFactor; // factor for step size
b = getBest();
x[b].setComment(comBasPoi);
report(x[b], SUBITERATION);
per.perturb(x[b], ssf);
if (per.gotOptimum()) { // we are at the minimum
retFla = 1;
reportMinimum();
iterate = false;
} else {
restart = true; // to prevent a recalculation of the 0-th pt.
x[0] = per.getOptimalPoint();
x[0].setComment(com[10]);
report(x[0], MAINITERATION);
if (writeStepNumber()) {
x[0].setComment(comNewSte);
x[0] = increaseStepNumber(x[0]);
report(x[0], MAINITERATION);
}
step = 1;
}
break;
}
} while (iterate);
return retFla;
}
