55 lines
2.0 KiB
Python
Executable File
55 lines
2.0 KiB
Python
Executable File
from mathutils import *
|
|
|
|
#Fiting center of circle of radius 70 to observerd points
|
|
radius = 70.0
|
|
x = [30.0, 50.0, 110.0, 35.0, 45.0]
|
|
y = [68.0, -6.0, -20.0, 15.0, 97.0]
|
|
p = plot(y, xdata=x)[0]
|
|
num_samples = len(x)
|
|
w = [ 1.0] * num_samples
|
|
w = [0.1, 0.1, 1.0, 0.1, 1.0]
|
|
#I = MatrixUtils.createRealIdentityMatrix(len(w))
|
|
W = MatrixUtils.createRealDiagonalMatrix(w)
|
|
|
|
|
|
# the model function components are the distances to current estimated center,
|
|
# they should be as close as possible to the specified radius
|
|
class Model(MultivariateJacobianFunction):
|
|
def value(self, variables):
|
|
(cx,cy) = (variables.getEntry(0), variables.getEntry(1))
|
|
value = ArrayRealVector(num_samples)
|
|
jacobian = Array2DRowRealMatrix(num_samples, 2)
|
|
for i in range(num_samples):
|
|
model = math.hypot(cx-x[i], cy-y[i])
|
|
value.setEntry(i, model)
|
|
# derivative with respect to p0 = x center
|
|
jacobian.setEntry(i, 0, (cx - x[i]) / model)
|
|
# derivative with respect to p1 = y center
|
|
jacobian.setEntry(i, 1, (cy - y[i]) / model)
|
|
return Pair(value, jacobian)
|
|
|
|
model = Model()
|
|
|
|
# the target is to have all points at the specified radius from the center
|
|
target = [radius,] * num_samples
|
|
|
|
#least squares problem to solve : modeled radius should be close to target radius
|
|
initial = [0.0, 0.0 ]
|
|
problem = LeastSquaresBuilder().start(initial).model(model).target(target).lazyEvaluation(False).maxEvaluations(1000).maxIterations(1000).weight(W).build()
|
|
optimizer = LevenbergMarquardtOptimizer()
|
|
optimum = optimizer.optimize(problem)
|
|
cx, cy = optimum.getPoint().getEntry(0), optimum.getPoint().getEntry(1)
|
|
|
|
print "fitted center x: ", cx
|
|
print "fitted center y: ", cy
|
|
print ""
|
|
print "RMS: " , optimum.getRMS()
|
|
print "evaluations: " , optimum.getEvaluations()
|
|
print "iterations: " , optimum.getIterations()
|
|
|
|
from plotutils import *
|
|
|
|
plot_point(p, cx, cy, name="Fit Cente")
|
|
plot_circle(p, cx, cy, radius, name="Fit")
|
|
|