From 8a4ecd5e1d67935590ce91d15c7f7737acaedf4a Mon Sep 17 00:00:00 2001 From: Val Erastov Date: Thu, 25 Sep 2014 22:42:29 -0700 Subject: [PATCH] multi solve --- src/cad/fx/App2DCtrl.java | 147 +++++++++++++++++++++------ src/cad/gcs/Solver.java | 31 +++--- src/cad/gcs/constr/Equals.java | 11 +- src/cad/gcs/constr/P2LDistance.java | 12 ++- src/cad/gcs/constr/Reconcilable.java | 7 ++ 5 files changed, 161 insertions(+), 47 deletions(-) create mode 100644 src/cad/gcs/constr/Reconcilable.java diff --git a/src/cad/fx/App2DCtrl.java b/src/cad/fx/App2DCtrl.java index 387443ad..5d4bef82 100644 --- a/src/cad/fx/App2DCtrl.java +++ b/src/cad/fx/App2DCtrl.java @@ -7,6 +7,7 @@ import cad.gcs.Solver; import cad.gcs.constr.P2LDistance; import cad.gcs.constr.Parallel; import cad.gcs.constr.Perpendicular; +import cad.gcs.constr.Reconcilable; import cad.math.Vector; import gnu.trove.list.TDoubleList; import javafx.application.Platform; @@ -16,17 +17,24 @@ import javafx.scene.Group; import javafx.scene.control.Button; import javafx.scene.layout.Pane; import javafx.scene.shape.Line; -import org.apache.commons.math3.analysis.MultivariateVectorFunction; +import org.apache.commons.math3.analysis.MultivariateFunction; +import org.apache.commons.math3.exception.MathIllegalStateException; +import org.apache.commons.math3.optim.ConvergenceChecker; import org.apache.commons.math3.optim.InitialGuess; import org.apache.commons.math3.optim.MaxEval; import org.apache.commons.math3.optim.MaxIter; +import org.apache.commons.math3.optim.PointValuePair; import org.apache.commons.math3.optim.PointVectorValuePair; +import org.apache.commons.math3.optim.SimpleBounds; +import org.apache.commons.math3.optim.nonlinear.scalar.GoalType; +import org.apache.commons.math3.optim.nonlinear.scalar.ObjectiveFunction; +import org.apache.commons.math3.optim.nonlinear.scalar.ObjectiveFunctionGradient; +import org.apache.commons.math3.optim.nonlinear.scalar.gradient.NonLinearConjugateGradientOptimizer; import org.apache.commons.math3.optim.nonlinear.vector.ModelFunction; import org.apache.commons.math3.optim.nonlinear.vector.ModelFunctionJacobian; import org.apache.commons.math3.optim.nonlinear.vector.Target; import org.apache.commons.math3.optim.nonlinear.vector.Weight; import org.apache.commons.math3.optim.nonlinear.vector.jacobian.LevenbergMarquardtOptimizer; -import org.jacop.constraints.Sum; import java.net.URL; import java.util.ArrayList; @@ -35,9 +43,9 @@ import java.util.List; import java.util.ResourceBundle; import java.util.concurrent.ExecutorService; import java.util.concurrent.Executors; -import java.util.concurrent.ThreadPoolExecutor; import static java.util.Arrays.asList; +import static org.apache.commons.math3.optim.nonlinear.scalar.gradient.NonLinearConjugateGradientOptimizer.Formula.FLETCHER_REEVES; public class App2DCtrl implements Initializable { @@ -187,7 +195,7 @@ public class App2DCtrl implements Initializable { executor.execute(() -> { globalSolve(subSystem, () -> Platform.runLater(update)); if (true) return; - while (subSystem.error() > 0.0001) { + while (subSystem.errorSquared() > 0.0001) { // Solver.solve_LM(subSystem); solveLM_COMMONS(subSystem); // Solver.solve_DL(subSystem); @@ -233,23 +241,54 @@ public class App2DCtrl implements Initializable { private void globalSolve(Solver.SubSystem subSystem, Runnable linearSolvedCallback) { +// for (Constraint c : subSystem.constraints) { +// if (c instanceof Reconcilable) { +// ((Reconcilable) c).reconcile(); +// } +// } + double eps = 0.0001; - while (subSystem.error() > eps) { + while (subSystem.errorSquared() > eps) { solveLM_COMMONS(subSystem); -// Solver.solve_LM(subSystem); - TDoubleList residuals = subSystem.calcResidual(); - double worseValue = residuals.max(); - if (Math.abs(worseValue) > eps) { - int worseId = residuals.indexOf(worseValue); - Solver.SubSystem worse = new Solver.SubSystem(asList(subSystem.constraints.get(worseId))); - solveLM_COMMONS(worse); -// Solver.solve_LM(worse); - System.out.println("WORSE FIXED ERROR:" + Math.sqrt(worse.error())); +// Solver.solve_LM(subSystem); + if (Math.abs(subSystem.errorSquared()) > eps) { +// solveWorse(subSystem, eps); + if(subSystem.constraints.size() > 1) { + Solver.SubSystem shrunk = shrink(subSystem); + globalSolve(shrunk, linearSolvedCallback); + } } linearSolvedCallback.run(); } } + private Solver.SubSystem shrink(Solver.SubSystem system) { + TDoubleList residuals = system.calcResidual(); + int minIdx = residuals.indexOf(residuals.min()); + ArrayList constrs = new ArrayList<>(system.constraints); + constrs.remove(minIdx); + return new Solver.SubSystem(constrs); + } + + private void solveWorse(Solver.SubSystem subSystem, double eps) { + TDoubleList residuals = subSystem.calcResidual(); + double worseValue = residuals.max(); + if (Math.abs(worseValue) > eps) { + int worseId = residuals.indexOf(worseValue); + Constraint worseConstr = subSystem.constraints.get(worseId); + if (worseConstr instanceof Reconcilable) { + ((Reconcilable) worseConstr).reconcile(); + } else { + Solver.SubSystem worse = new Solver.SubSystem(asList(worseConstr)); + solveLM_COMMONS(worse); +// Solver.solve_LM(worse); + + } + + System.out.println("WORSE FIXED ERROR:" + worseConstr.error()); + } + } + double xxx = 100; private void scale(Line l) { @@ -263,6 +302,48 @@ public class App2DCtrl implements Initializable { l.setEndY(l.getStartY() + v.y); } + private void solveScalarFunc(final Solver.SubSystem subSystem) { + double eps = 1e-10; + ConvergenceChecker convergenceChecker = new ConvergenceChecker() { + @Override + public boolean converged(int iteration, PointValuePair previous, PointValuePair current) { + return previous.getValue() < eps; + } + }; + NonLinearConjugateGradientOptimizer optimizer = new NonLinearConjugateGradientOptimizer(FLETCHER_REEVES, convergenceChecker); + double[] lb = new double[subSystem.pSize()]; + double[] ub = new double[subSystem.pSize()]; + + Arrays.fill(lb, -1000); + Arrays.fill(ub, 1000); + + optimizer.optimize( + new MaxEval(10000), + new InitialGuess(subSystem.getParams().toArray()), + GoalType.MINIMIZE, + new SimpleBounds(lb, ub), +// new NonLinearConjugateGradientOptimizer.BracketingStep( 100 ), + getGradient(subSystem), + getScalarFunction(subSystem)); + } + + private ObjectiveFunction getScalarFunction(Solver.SubSystem system) { + return new ObjectiveFunction(point -> { + system.setParams(point); + return system.value(); + }); + } + + private ObjectiveFunctionGradient getGradient(Solver.SubSystem subSystem) { + return new ObjectiveFunctionGradient(point -> { + subSystem.setParams(point); + Constraint constraint = subSystem.constraints.get(0); + double[] out = new double[constraint.pSize()]; + constraint.gradient(out); + return out; + }); + } + private void solveLM_COMMONS(final Solver.SubSystem subSystem) { double eps = 1e-10, eps1 = 1e-80; double tau = 1e-3; @@ -272,28 +353,32 @@ public class App2DCtrl implements Initializable { double[] wieght = new double[subSystem.cSize()]; Arrays.fill(wieght, 1); PointVectorValuePair result = optimizer.optimize( - new MaxEval(10000), - new MaxIter(10000), - new InitialGuess(subSystem.getParams().toArray()), - new Target(new double[subSystem.cSize()]), - new Weight(wieght), - new ModelFunctionJacobian(point -> { - subSystem.setParams(point); - return subSystem.makeJacobi().getData(); - }), - new ModelFunction(new MultivariateVectorFunction() { - @Override - public double[] value(double[] point) throws IllegalArgumentException { - subSystem.setParams(point); - return subSystem.getValues().toArray(); - } - }) - + new MaxEval(10000), + new MaxIter(10000), + new InitialGuess(subSystem.getParams().toArray()), + new Target(new double[subSystem.cSize()]), + new Weight(wieght), + getJacobian(subSystem), + getFunction(subSystem) ); subSystem.setParams(result.getPoint()); } + private ModelFunction getFunction(Solver.SubSystem subSystem) { + return new ModelFunction(point -> { + subSystem.setParams(point); + return subSystem.getValues().toArray(); + }); + } + + private ModelFunctionJacobian getJacobian(Solver.SubSystem subSystem) { + return new ModelFunctionJacobian(point -> { + subSystem.setParams(point); + return subSystem.makeJacobi().getData(); + }); + } + private void solve(ActionEvent e) { // UnconstrainedLeastSquares opt = FactoryOptimization.leastSquaresTrustRegion(100, RegionStepType.DOG_LEG_FTF, false); diff --git a/src/cad/gcs/Solver.java b/src/cad/gcs/Solver.java index d9375e8d..c2520fc0 100644 --- a/src/cad/gcs/Solver.java +++ b/src/cad/gcs/Solver.java @@ -301,7 +301,7 @@ public class Solver { // setCost(computeCost(currentResiduals)); // return current; // } - if (subSystem.value() < 0.0001) { + if (subSystem.valueSquared() < 0.0001) { return; } } @@ -536,19 +536,19 @@ public class Solver { //Start at the initial position alpha1 = 0 alpha1 = 0.; - f1 = subsys.error(); + f1 = subsys.errorSquared(); //Take a step of alpha2 = 1 alpha2 = 1.; x = x0.add(xdir.scalarMultiply(alpha2)); subsys.setParams(x); - f2 = subsys.error(); + f2 = subsys.errorSquared(); //Take a step of alpha3 = 2*alpha2 alpha3 = alpha2 * 2; x = x0.add(xdir.scalarMultiply(alpha3)); subsys .setParams(x); - f3 = subsys . error(); + f3 = subsys .errorSquared(); //Now reduce or lengthen alpha2 and alpha3 until the minimum is //Bracketed by the triplet f1>f2 f3) { if (alpha3 >= alphaMax) { break; @@ -573,7 +573,7 @@ public class Solver { alpha3 = alpha3 * 2; x = x0.add( xdir.scalarMultiply(alpha3)); subsys . setParams(x); - f3 = subsys . error(); + f3 = subsys .errorSquared(); } } //Get the alpha for the minimum f of the quadratic approximation @@ -623,7 +623,7 @@ public class Solver { // Initial search direction oposed to gradient (steepest-descent) xdir = grad.scalarMultiply(-1); lineSearch(subsys, xdir); - double err = subsys.error(); + double err = subsys.errorSquared(); h = new Array2DRowRealMatrix(x.getData()); subsys.fillParams(x); @@ -667,7 +667,7 @@ public class Solver { xdir = D.scalarMultiply(-1).multiply(grad); lineSearch(subsys, xdir); - err = subsys.error(); + err = subsys.errorSquared(); h = new Array2DRowRealMatrix(x.getData()); subsys.fillParams(x); @@ -800,7 +800,7 @@ public class Solver { return r; } - public double value() { + public double valueSquared() { double err = 0.; for (Constraint c : constraints) { double v = c.error(); @@ -810,6 +810,14 @@ public class Solver { return err; } + public double value() { + double err = 0.; + for (Constraint c : constraints) { + err += c.error(); + } + return err; + } + public void calcJacobi(RealMatrix jacobi) { // jacobi.setZero(csize, params.size()); @@ -839,7 +847,6 @@ public class Solver { return jacobi; } - public void setParams(RealMatrix params) { setParams(params.getColumn(0)); } @@ -851,8 +858,8 @@ public class Solver { } } - public double error() { - return value(); + public double errorSquared() { + return valueSquared(); } public void calcGrad(RealMatrix out) { diff --git a/src/cad/gcs/constr/Equals.java b/src/cad/gcs/constr/Equals.java index 12a643f8..62f35181 100644 --- a/src/cad/gcs/constr/Equals.java +++ b/src/cad/gcs/constr/Equals.java @@ -3,7 +3,7 @@ package cad.gcs.constr; import cad.gcs.Constraint; import cad.gcs.Param; -public class Equals implements Constraint { +public class Equals implements Constraint, Reconcilable { private final Param[] params; @@ -31,4 +31,13 @@ public class Equals implements Constraint { public int pSize() { return params.length; } + + @Override + public void reconcile() { + double x1 = params[0].get(); + double x2 = params[1].get(); + double diff = (x1 - x2) / 2; + params[0].set(x1 - diff); + params[1].set(x2 + diff); + } } diff --git a/src/cad/gcs/constr/P2LDistance.java b/src/cad/gcs/constr/P2LDistance.java index e9977357..c77772b5 100644 --- a/src/cad/gcs/constr/P2LDistance.java +++ b/src/cad/gcs/constr/P2LDistance.java @@ -34,6 +34,9 @@ public class P2LDistance implements Constraint { double d = sqrt(dx * dx + dy * dy); double area = abs (-x0 * dy + y0 * dx + x1 * y2 - x2 * y1); // = x1y2 - x2y1 - x0y2 + x2y0 + x0y1 - x1y0 = 2*(triangle area) + if (d == 0) { + return 0; + } return (area / d - dist); } @@ -172,9 +175,12 @@ public class P2LDistance implements Constraint { out[lp2x] = (((y0 - y1) * d - (dx / d) * area) / d2); out[lp2y] = (((x1 - x0) * d - (dy / d) * area) / d2); - if (area < 0) { - for (int i = 0; i < 6; i++) { - out[i] *= -1; + for (int i = 0; i < 6; i++) { + if (Double.isNaN(out[i])) { + out[i] = 0; + } + if (area < 0) { + out[i] *= -1; } } } diff --git a/src/cad/gcs/constr/Reconcilable.java b/src/cad/gcs/constr/Reconcilable.java new file mode 100644 index 00000000..d929f3e0 --- /dev/null +++ b/src/cad/gcs/constr/Reconcilable.java @@ -0,0 +1,7 @@ +package cad.gcs.constr; + +public interface Reconcilable { + + public void reconcile(); + +}