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;
}
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);
}
