diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000..1c40a0f --- /dev/null +++ b/.travis.yml @@ -0,0 +1,15 @@ +language: python +sudo: false +python: + - 2.7 +branches: + only: + - master + - develop +install: + - "pip install coverage" + - "pip install -r requirements.txt" + - "python setup.py build" + - "cp build/lib.linux-x86_64-2.7/b2ac/ext/ellipse_fitter.so b2ac/ext/ellipse_fitter.so" +script: nosetests tests --with-coverage --cover-package=b2ac + diff --git a/README.md b/README.md index 4d9976f..6126852 100644 --- a/README.md +++ b/README.md @@ -1,55 +1,121 @@ # B2AC - Ellipse fitting in Python and C +| Branch | Build status | +| :------------ | ---------------: | +| `master` | [![Build Status](https://travis-ci.org/hbldh/b2ac.svg?branch=master)](https://travis-ci.org/hbldh/b2ac) | +| `develop` | [![Build Status](https://travis-ci.org/hbldh/b2ac.svg?branch=develop)](https://travis-ci.org/hbldh/b2ac) | + Python and C implementations of an ellipse fitting algorithm in double and fixed point precision. The implementations here were never meant for production, but were written as an examination of how much speedup could be attained from implementing ellipse fitting methods in fixed point and at what precision cost. -The ellipse fitting method in (2) (an improved version of the one in (1)) was implemented. - -### Solution strategy for double precision - -TBD. - -### Solution strategy for fixed point precision - -TBD. - -#### Limitations for fixed point implementation. - -TBD. +The ellipse fitting method in \[2\] (an improved version of the one in \[1\]) was implemented. ## Installation Install simply by calling: - pip install https://github.com/hbldh/ellipse-fitting + pip install git+https://www.github.com/hbldh/b2ac Numpy needs to be installed prior to `b2ac`, since it compiles the C extension using Numpy headers. ## Testing -Tests will be eventually be implemented. +Test with nosetests: + + nosetests tests ## Usage +An ellipse can be fitted using Python methods: + ```python -import b2ac.ellipse +import numpy as np +import b2ac.preprocess +import b2ac.fit +import b2ac.conversion + +points = np.array([[3, 0], [0, 5], [3, 10], [6, 5]]) + +# Optional removal of mean value from points to obtain better +# fitting performance, especially in integer precision. +points, x_mean, y_mean = b2ac.preprocess.remove_mean_values(points) + +# Fit using NumPy methods in double precision. +conic_numpy = b2ac.fit.fit_improved_B2AC_numpy(points) +# Fit using own written methods in double precision. +conic_double = b2ac.fit.fit_improved_B2AC_double(points) +# Fit using own written methods in 64-bit integer precision. +conic_int = b2ac.fit.fit_improved_B2AC_int(points) + +# Convert from conic coefficient form to general ellipse form. +general_form_numpy = b2ac.conversion.conic_to_general_1(conic_numpy) +general_form_numpy[0][0] + x_mean +general_form_numpy[0][1] + y_mean + +general_form_double = b2ac.conversion.conic_to_general_1(conic_double) +general_form_double[0][0] + x_mean +general_form_double[0][1] + y_mean -ellipse = b2ac.ellipse.def fit_improved_B2AC_double(points) +general_form_int = b2ac.conversion.conic_to_general_int(conic_int) +general_form_int[0][0] + x_mean +general_form_int[0][1] + y_mean ``` +> The mathematical notation described below follows the one used in \[2\]. + +### Solution strategy + +First, the scatter matrix **S** is calculated according to regular specifications, +and the **M** matrix as well. The inverse of **S3** is calculated through +definition of [3x3 matrix inverse](http://mathworld.wolfram.com/MatrixInverse.html). + +A QR algorithm with largest value shift, using Givens rotations for both +tridiagonalization and actual QR steps, (See \[3\]) is then used +to find eigenvalues of the matrix **M**. With these eigenvalues, we +apply an inverse iteration method for calculating the +corresponding eigenvectors. + +The algorithms returns the conic coefficients defining one fitted ellipse. +These can then be transformed to general form: center point, +axes lengths and rotation angle. + +#### Special considerations for integer version + +> The integer version uses 64 bit integers! + +The calculations of **S** and **M** has special handling. The fact that +this method uses a lot of squared values can easily lead to overflow, especially if +32 bit integers are used. + +The major thing to remember here is that when calculating inverses, +the division of the determinant is postponed, thus using the determinant as +a scale factor during calculations. This scale factor can then be removed +first when the sought eigenvector has been found. + +#### Limitations for fixed point implementation + +For integer versions, it is of great importance to first remove the +mean values from the points, thus making all values much smaller. This will +drastically improve the fitting results. + +Another important aspect is the number of points used for estimating an +ellipse. Using `>100` points, the matrix values in the estimation also grows +and the possibility of overflow is increased. Subsampling of points is recommended. + + ## References -1. [Fitzgibbon, A.; Pilu, M.; Fisher, R.B., +1. [Fitzgibbon, A., Pilu, M., & Fisher, R.B., "Direct least square fitting of ellipses," Pattern Analysis and Machine Intelligence, IEEE Transactions on , vol.21, no.5, pp.476-480, May 1999] (http://research.microsoft.com/pubs/67845/ellipse-pami.pdf) -2. [Hal?r, R., & Flusser, J.; "Numerically stable direct least squares fitting of ellipses." +2. [Halir, R., & Flusser, J., "Numerically stable direct least squares fitting of ellipses." Proc. 6th International Conference in Central Europe on Computer Graphics and Visualization. WSCG. Vol. 98. 1998](http://autotrace.sourceforge.net/WSCG98.pdf) -3. Golub, G.H. & Van Loan, C.F.; 2012. Matrix Computations (4th Ed.). - Johns Hopkins University Press, Baltimore, MD, USA. \ No newline at end of file +3. Golub, G.H. & Van Loan, C.F., 2012. Matrix Computations (4th Ed.). + Johns Hopkins University Press, Baltimore, MD, USA. diff --git a/b2ac/__init__.py b/b2ac/__init__.py index 66339bd..a5682fb 100644 --- a/b2ac/__init__.py +++ b/b2ac/__init__.py @@ -1 +1,2 @@ -__author__ = 'henri' +#!/usr/bin/env python +# -*- coding: utf-8 -*- \ No newline at end of file diff --git a/b2ac/conversion.py b/b2ac/conversion.py new file mode 100644 index 0000000..6c1cf69 --- /dev/null +++ b/b2ac/conversion.py @@ -0,0 +1,306 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +:mod:`conversion` +================== + +.. module:: conversion + :platform: Unix, Windows + :synopsis: + +.. moduleauthor:: hbldh + +Created on 2015-10-07 + +""" + +from __future__ import division +from __future__ import print_function +from __future__ import unicode_literals +from __future__ import absolute_import + +import numpy as np + +import b2ac.matrix.matrix_algorithms as ma + + +def conic_to_general_1(conic_coeffs, verbose=False): + """Transform from conic section format to general format. + + Adopted from http://math.stackexchange.com/a/423272 + + :param conic_coeffs: The six coefficients defining the ellipse as a conic shape in + :math:`ax^2 + bxy + cy^2 + dx + ey + f = 0`. + :type conic_coeffs: :py:class:`numpy.ndarray` or tuple + :return: The general form for the ellipse. Returns tuple :math:`(x_c,\ y_c),\\ (a,\\ b),\\ \\theta` + that fulfills the equation + + .. math:: + + \\frac{((x-x_c)\\cos(\\theta) + (y-y_c)\\sin(\\theta))^2}{a^2} + + \\frac{((x-x_c)\\sin(\\theta) - (y-y_c)\\sin(\\theta))^2}{b^2} = 1 + + :rtype: tuple + + """ + a, b, c, d, e, f = conic_coeffs + + angle = np.arctan2(b, a - c) / 2 + + # Obtaining angles using Givens rotations. + # TODO: Evaluate this method of calculating angles. + # cos_2t, sin_2t = ma.Givens_rotation_double(b, a-c) + # cos_2t, sin_2t = -sin_2t, cos_2t + # + # if cos_2t < 0: + # sin_t = np.sqrt((1 - cos_2t)/2) + # cos_t = sin_2t / (2 * sin_t) + # else: + # cos_t = np.sqrt((cos_2t + 1)/2) + # sin_t = sin_2t / (2 * cos_t) + + cos_t = np.cos(angle) + sin_t = np.sin(angle) + + a_prime = a * (cos_t ** 2) + b * cos_t * sin_t + c * (sin_t ** 2) + c_prime = a * (sin_t ** 2) - b * cos_t * sin_t + c * (cos_t ** 2) + d_prime = (d * cos_t) + (e * sin_t) + e_prime = (-(d * sin_t)) + (e * cos_t) + f_prime = f + + x_prime = (-d_prime) / (2 * a_prime) + y_prime = (-e_prime) / (2 * c_prime) + x = (x_prime * cos_t) - (y_prime * sin_t) + y = (x_prime * sin_t) + (y_prime * cos_t) + + maj_axis = np.sqrt(((-4 * f_prime * a_prime * c_prime) + + (c_prime * (d_prime ** 2)) + + (a_prime * (e_prime ** 2))) / (4 * a_prime * (c_prime ** 2))) + min_axis = np.sqrt(((-4 * f_prime * a_prime * c_prime) + + (c_prime * (d_prime ** 2)) + + (a_prime * (e_prime ** 2))) / (4 * (a_prime ** 2) * c_prime)) + if np.isnan(maj_axis): + maj_axis = 0.0 + if np.isnan(min_axis): + min_axis = 0.0 + + if a_prime > c_prime: + x_axis = min_axis + y_axis = maj_axis + else: + x_axis = maj_axis + y_axis = min_axis + + general_coeffs = [x, y], [x_axis, y_axis], angle + + if verbose: + print("Cos: {0}, Sin: {1}".format(cos_t, sin_t)) + print("Conic form = {0:.4f}x^2 + {1:.4f}xy + " + "{2:.4f}y^2 + {3:.4f}x + {4:.4f}y + {5:.4f}".format(*conic_coeffs)) + print("Conic form 2 = {0:.4f}x^2 + {1:.4f}xy + " + "{2:.4f}y^2 + {3:.4f}x + {4:.4f}y + {5:.4f}".format(a_prime, 0, c_prime, + d_prime, e_prime, f_prime)) + print("Elliptical form: Center = {0}, Radii = {1}, Angle = {2}\n".format(*general_coeffs)) + + return general_coeffs + + +def conic_to_general_2(conic_coeffs, verbose=False): + """Transform from conic section format to general format. + + :param conic_coeffs: The six coefficients defining the ellipse as a conic shape. + :type conic_coeffs: :py:class:`numpy.ndarray` or tuple + :return: The general form for the ellipse. Returns tuple :math:`(x_c,\ y_c),\\ (a,\\ b),\\ \\theta` + that fulfills the equation + + .. math:: + + \\frac{((x-x_c)\\cos(\\theta) + (y-y_c)\\sin(\\theta))^2}{a^2} + + \\frac{((x-x_c)\\sin(\\theta) - (y-y_c)\\sin(\\theta))^2}{b^2} = 1 + + :rtype: tuple + + """ + a, b, c, d, e, f = conic_coeffs + denom = 2 * ((b ** 2) - (a * c)) + x = (c * d - b * e) / denom + y = (a * e - b * d) / denom + mu = 1 / ((a * (x ** 2)) + (2 * b * x * y) + (c * (y ** 2)) - f) + + sqrt_expr = np.sqrt(((mu * a - mu * c) ** 2) + (4 * ((mu * b) ** 2))) + min_axis = 1 / np.sqrt((mu * a + mu * c + sqrt_expr) / 2) + maj_axis = 1 / np.sqrt((mu * a + mu * c - sqrt_expr) / 2) + angle = np.arctan2(-2 * b, c - a) + + return [x, y], [maj_axis, min_axis], angle + + +def conic_to_general_3(conic_coeffs, verbose=False): + """Transform from conic section format to general format. + + :param conic_coeffs: The six coefficients defining the ellipse as a conic shape. + :type conic_coeffs: :py:class:`numpy.ndarray` or tuple + :param verbose: If debug printout is desired. + :type verbose: bool + :return: + :rtype: tuple + + """ + a, b, c, d, e, f = conic_coeffs + + angle = np.arctan2(b, a - c) / 2 + if a > c: + angle += np.pi / 2 + cos_theta = np.cos(angle) # np.sqrt((1 + np.cos(2*angle)) / 2) + sin_theta = np.sin(angle) # np.sqrt((1 - np.cos(2*angle)) / 2) + + a_prim = a * (cos_theta ** 2) + (b * cos_theta * sin_theta) + c * (sin_theta ** 2) + c_prim = a * (sin_theta ** 2) - (b * cos_theta * sin_theta) + c * (cos_theta ** 2) + d_prim = d * cos_theta + e * sin_theta + e_prim = -(d * sin_theta) + e * cos_theta + f_prim = f + + x_prim = (-d_prim) / (2 * a_prim) + y_prim = (-e_prim) / (2 * c_prim) + a_squared = (((-4 * f_prim * a_prim * c_prim) + (c_prim * (d_prim ** 2)) + (a_prim * (e_prim ** 2))) / + (4 * a_prim * (c_prim ** 2))) + b_squared = (((-4 * f_prim * a_prim * c_prim) + (c_prim * (d_prim ** 2)) + (a_prim * (e_prim ** 2))) / + (4 * (a_prim ** 2) * c_prim)) + + x = x_prim * cos_theta - y_prim * sin_theta + y = x_prim * sin_theta + y_prim * cos_theta + + return [x, y], [a_squared, b_squared], angle + + +def conic_to_general_int(conic_coeffs, return_float=False, verbose=False): + """Transform from conic section format to general format, in integer precision. + + :param conic_coeffs: The six coefficients defining the ellipse as a conic shape in + :math:`ax^2 + bxy + cy^2 + dx + ey + f = 0`. + :type conic_coeffs: :py:class:`numpy.ndarray` or tuple + :return: The general form for the ellipse. Returns tuple :math:`(x_c,\ y_c),\\ (a,\\ b),\\ \\theta` + that fulfills the equation + + .. math:: + + \\frac{((x-x_c)\\cos(\\theta) + (y-y_c)\\sin(\\theta))^2}{a^2} + + \\frac{((x-x_c)\\sin(\\theta) - (y-y_c)\\sin(\\theta))^2}{b^2} = 1 + + :rtype: tuple + + """ + # These conic coefficients have a scaling e_norm = sqrt(a**2 + b**2 + c**2) from + # the eigenvector not being normalized before turning it into conic coefficients. + a, b, c, d, e, f = conic_coeffs + angle = np.arctan2(b, (a - c)) / 2 + + if b == 0: + if (a-c) < 0: + unity = 1 + cos_t = 0 + sin_t = 1 + else: + unity = 1 + cos_t = 1 + sin_t = 0 + elif (a-c) == 0: + if b < 0: + unity = 50 + cos_t = 5 + sin_t = -5 + else: + unity = 50 + cos_t = 5 + sin_t = 5 + else: + # unity here becomes a value with scaling e_norm. + cos_2t, sin_2t, unity = ma.Givens_rotation_int(b, (a-c)) + + cos_2t, sin_2t = -sin_2t, cos_2t + + if cos_2t < 0: + sin_t = ma.sqrt_int64(int((unity - cos_2t) >> 1), False) # sin_t is scaled up sqrt(unity) + cos_t = sin_2t // (2 * sin_t) # cos_t also becomes sqrt(unity) scaled + # Now that we have cos and sin values we establish a new scaling with + # unity value obtained through the sin^2(x) + cos^2(x) = 1 trigonometric identity. + unity = sin_t ** 2 + cos_t ** 2 + + else: + cos_t = ma.sqrt_int64(int((cos_2t + unity) >> 1), False) # cos_t is scaled up sqrt(unity) + sin_t = sin_2t // (2 * cos_t) # sin_t also becomes sqrt(unity) scaled + # Now that we have cos and sin values we establish a new scaling with + # unity value obtained through the sin^2(x) + cos^2(x) = 1 trigonometric identity. + unity = sin_t ** 2 + cos_t ** 2 + + a_prime = a * (cos_t ** 2) + b * cos_t * sin_t + c * (sin_t ** 2) # Is unity + e_norm scaled + c_prime = a * (sin_t ** 2) - b * cos_t * sin_t + c * (cos_t ** 2) # Is unity + e_norm scaled + d_prime = (d * cos_t) + (e * sin_t) # Is sqrt(unity) + e_norm scaled + e_prime = (-(d * sin_t)) + (e * cos_t) # Is sqrt(unity) + e_norm scaled + f_prime = f # Is e_norm scaled only. + + x_prime_num = (-d_prime) + x_prime_denom = (2 * a_prime) + + y_prime_num = (-e_prime) + y_prime_denom = (2 * c_prime) + + if return_float: + # At sub-pixel precision, treat values as floats. + x = (((x_prime_num * cos_t) / x_prime_denom) - ((y_prime_num * sin_t) / y_prime_denom)) + y = (((x_prime_num * sin_t) / x_prime_denom) + ((y_prime_num * cos_t) / y_prime_denom)) + else: + # At pixel precision, perform integer division. + x = int(((x_prime_num * cos_t) // x_prime_denom) - + ((y_prime_num * sin_t) // y_prime_denom)) + y = int(((x_prime_num * sin_t) // x_prime_denom) + + ((y_prime_num * cos_t) // y_prime_denom)) + + sqrt_unity = ma.sqrt_int64(int(unity)) + + a_prime //= unity + c_prime //= unity + d_prime //= sqrt_unity + e_prime //= sqrt_unity + + if return_float: + # At sub-pixel precision, use float divison and square root. + numerator = ((-4 * f_prime * a_prime * c_prime) + + (c_prime * (d_prime ** 2)) + + (a_prime * (e_prime ** 2))) + denominator_major = (4 * a_prime * (c_prime ** 2)) + tmp_axis = ma.sqrt_int64((numerator // denominator_major), True) + maj_axis = tmp_axis[0] + tmp_axis[1] + denominator_minor = (4 * (a_prime ** 2) * c_prime) + tmp_axis = ma.sqrt_int64(numerator // denominator_minor, True) + min_axis = tmp_axis[0] + tmp_axis[1] + else: + # At pixel precision, use integer division and perform + # fixed point square root. + numerator = ((-4 * f_prime * a_prime * c_prime) + + (c_prime * (d_prime ** 2)) + + (a_prime * (e_prime ** 2))) * unity + denominator_major = (4 * a_prime * (c_prime ** 2)) + maj_axis = ma.sqrt_int64(int(numerator // denominator_major)) + denominator_minor = (4 * (a_prime ** 2) * c_prime) + min_axis = ma.sqrt_int64(int(numerator // denominator_minor)) + + if a_prime > c_prime: + x_axis = min_axis + y_axis = maj_axis + else: + x_axis = maj_axis + y_axis = min_axis + + general_coeffs = [x, y], [x_axis, y_axis], angle + + if verbose: + print("Cos: {0} => {1}, Sin: {2} => {3}".format(cos_t, cos_t / np.sqrt(unity), sin_t, sin_t / np.sqrt(unity))) + print("Conic form = {0:.4f}x^2 + {1:.4f}xy + " + "{2:.4f}y^2 + {3:.4f}x + {4:.4f}y + {5:.4f}".format(*conic_coeffs)) + print("Conic form 2 = {0:.4f}x^2 + {1:.4f}xy + " + "{2:.4f}y^2 + {3:.4f}x + {4:.4f}y + {5:.4f}".format( + a_prime * unity, 0, c_prime * unity, d_prime * np.sqrt(unity), e_prime * np.sqrt(unity), f_prime)) + print("Elliptical form: Center = {0}, Radii = {1}, Angle = {2}\n".format(*general_coeffs)) + + return general_coeffs diff --git a/b2ac/eigenmethods/__init__.py b/b2ac/eigenmethods/__init__.py index cb66d2d..a5682fb 100644 --- a/b2ac/eigenmethods/__init__.py +++ b/b2ac/eigenmethods/__init__.py @@ -1 +1,2 @@ -__author__ = 'Henrik Blidh' +#!/usr/bin/env python +# -*- coding: utf-8 -*- \ No newline at end of file diff --git a/b2ac/eigenmethods/inverse_iteration.py b/b2ac/eigenmethods/inverse_iteration.py index f9aadf6..173feee 100644 --- a/b2ac/eigenmethods/inverse_iteration.py +++ b/b2ac/eigenmethods/inverse_iteration.py @@ -43,7 +43,7 @@ def inverse_iteration_for_eigenvector_double(A, eigenvalue, n_iterations=1): """ A = np.array(A, 'float') # Subtract the eigenvalue from the diagonal entries of the matrix. - # N.B. Also slightly perturb the eigenvalue so the matrix will + # N_POLYPOINTS.B. Also slightly perturb the eigenvalue so the matrix will # not be so close to singular! for k in xrange(A.shape[0]): A[k, k] -= eigenvalue + 0.001 @@ -92,4 +92,4 @@ def inverse_iteration_for_eigenvector_int(A, eigenvalue): e_norm = int(np.sqrt((eigenvector ** 2).sum())) if (eigenvector[0] < 0) and (eigenvector[2] < 0): eigenvector = -eigenvector - return eigenvector, e_norm \ No newline at end of file + return eigenvector, e_norm diff --git a/b2ac/eigenmethods/qr_algorithm.py b/b2ac/eigenmethods/qr_algorithm.py index bf3a741..269f758 100644 --- a/b2ac/eigenmethods/qr_algorithm.py +++ b/b2ac/eigenmethods/qr_algorithm.py @@ -30,7 +30,7 @@ def QR_algorithm(A): """The straight QR algorithm for finding eigenvalues. - N.B. Has very slow convergence. Use shifted versions instead. + N_POLYPOINTS.B. Has very slow convergence. Use shifted versions instead. .. code-block:: matlab diff --git a/b2ac/ellipse.py b/b2ac/ellipse.py deleted file mode 100644 index 2e69fdb..0000000 --- a/b2ac/ellipse.py +++ /dev/null @@ -1,505 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -""" -.. module:: ellipse_fitting - :platform: Unix, Windows - :synopsis: Ellipse fitting algorithms and handling of ellipse information. - -.. moduleauthor:: hbldh - -Created on 2013-09-09, 16:32 - -""" - -from __future__ import division -from __future__ import print_function -from __future__ import unicode_literals -from __future__ import absolute_import - -import numpy as np - -import b2ac.matrix.matrix_operations as mo -import b2ac.matrix.matrix_algorithms as ma -import b2ac.eigenmethods.qr_algorithm as qr -import b2ac.eigenmethods.inverse_iteration as inv_iter - -DEBUG = True - - -def fit_B2AC(points): - """Ellipse fitting in Python with numerically unstable algorithm. - - Described `here `_. - - N.B. Do not use, since it works with almost singular matrix. - - :param points: The [Nx2] array of points to fit ellipse to. - :type points: :py:class:`numpy.ndarray` - :return: The conic section array defining the fitted ellipse. - :rtype: :py:class:`numpy.ndarray` - - """ - import scipy.linalg as scla - - constraint_matrix = np.zeros((6, 6)) - constraint_matrix[0, 2] = 2 - constraint_matrix[1, 1] = -1 - constraint_matrix[2, 0] = 2 - - S = _calculate_scatter_matrix_py(points[:, 0], points[:, 1]) - - eigenvalues, eigenvalues = scla.eig(S, constraint_matrix) - ind = np.where(eigenvalues == (eigenvalues[eigenvalues > 0].min()))[0][0] - return eigenvalues[:, ind] - - -def fit_improved_B2AC_double(points): - """Ellipse fitting in Python with improved B2AC algorithm as described in - this `paper `_. - - This version of the fitting uses float storage during calculations and performs the - eigensolver on a float array. - - :param points: The [Nx2] array of points to fit ellipse to. - :type points: :py:class:`numpy.ndarray` - :return: The conic section array defining the fitted ellipse. - :rtype: :py:class:`numpy.ndarray` - - """ - e_conds = [] - points, x_mean, y_mean = _remove_mean_values(points) - - M, T = _calculate_M_and_T_double(points) - - e_vals = sorted(qr.QR_algorithm_shift_Givens_double(M)[0]) - - a = None - for ev_ind in [1, 2, 0]: - # Find the eigenvector that matches this eigenvector. - eigenvector = inv_iter.inverse_iteration_for_eigenvector_double(M, e_vals[ev_ind], 5) - - # See if that eigenvector yields an elliptical solution. - elliptical_condition = (4 * eigenvector[0] * eigenvector[2]) - ((eigenvector[1] ** 2)) - e_conds.append(elliptical_condition) - if elliptical_condition > 0: - a = eigenvector - break - - if a is None: - print("Eigenvalues = {0}".format(e_vals)) - print("Elliptical conditions = {0}".format(e_conds)) - raise ArithmeticError("No elliptical solution found.") - - if DEBUG: - print("Elliptical solution = {0}: {1}".format(e_vals[ev_ind], a)) - - conic_coefficients = np.concatenate((a, np.dot(T, a))) - rotated_euclidian_coefficients = list(_general_to_rotated(conic_coefficients)) - rotated_euclidian_coefficients[0] = (rotated_euclidian_coefficients[0][0] + x_mean, - rotated_euclidian_coefficients[0][1] + y_mean) - return rotated_euclidian_coefficients - - -def fit_improved_B2AC_int(points): - """Ellipse fitting in Python with improved B2AC algorithm as described in - this `paper `_. - - This version of the fitting uses int64 storage during calculations and performs the - eigensolver on an integer array. - - :param points: The [Nx2] array of points to fit ellipse to. - :type points: :py:class:`numpy.ndarray` - :return: The conic section coefficients array defining the fitted ellipse. - :rtype: :py:class:`numpy.ndarray` - - """ - e_conds = [] - points, x_mean, y_mean = _remove_mean_values(points) - - M, T_no_det, determinant_S3 = _calculate_M_and_T_int64(points) - - e_vals = sorted(qr.QR_algorithm_shift_Givens_int(M)[0]) - - a = None - for ev_ind in [1, 2, 0]: - # Find the eigenvector that matches this eigenvector. - eigenvector, e_norm = inv_iter.inverse_iteration_for_eigenvector_int(M, e_vals[ev_ind]) - # See if that eigenvector yields an elliptical solution. - elliptical_condition = (4 * eigenvector[0] * eigenvector[2]) - ((eigenvector[1] ** 2)) - e_conds.append(elliptical_condition) - if elliptical_condition > 0: - a = eigenvector - break - - if a is None: - raise ArithmeticError("No elliptical solution found.") - - if DEBUG: - print("Elliptical solution = {0}: {1}".format(e_vals[ev_ind], a)) - - conic_coefficients = np.concatenate((a, np.dot(T_no_det, a) // determinant_S3)) - rotated_euclidian_coefficients = list(_general_to_rotated_int(conic_coefficients, True)) - rotated_euclidian_coefficients[0] = (rotated_euclidian_coefficients[0][0] + x_mean, - rotated_euclidian_coefficients[0][1] + y_mean) - return rotated_euclidian_coefficients - - -def _general_to_rotated(conic_coeffs): - """Transform from conic section format to general format. - - :param conic_coeffs: The six coefficients defining the ellipse as a conic shape in - :math:`ax^2 + bxy + cy^2 + dx + ey + f = 0`. - :type conic_coeffs: :py:class:`numpy.ndarray` or tuple - :return: The general form for the ellipse. Returns tuple :math:`(x_c,\ y_c),\\ (a,\\ b),\\ \\theta` - that fulfills the equation - - .. math:: - - \\frac{((x-x_c)\\cos(\\theta) + (y-y_c)\\sin(\\theta))^2}{a^2} + - \\frac{((x-x_c)\\sin(\\theta) - (y-y_c)\\sin(\\theta))^2}{b^2} = 1 - - :rtype: tuple - - """ - a, b, c, d, e, f = conic_coeffs - - angle = np.arctan2(b, a - c) / 2 - - # Obtaining angels using Givens rotations. - # cos_2t, sin_2t = ma.Givens_rotation_double(b, a-c) - # cos_2t, sin_2t = -sin_2t, cos_2t - # - # if cos_2t < 0: - # sin_t = np.sqrt((1 - cos_2t)/2) - # cos_t = sin_2t / (2 * sin_t) - # else: - # cos_t = np.sqrt((cos_2t + 1)/2) - # sin_t = sin_2t / (2 * cos_t) - cos_t = np.cos(angle) - sin_t = np.sin(angle) - - a_prime = a * (cos_t ** 2) + b * cos_t * sin_t + c * (sin_t ** 2) - c_prime = a * (sin_t ** 2) - b * cos_t * sin_t + c * (cos_t ** 2) - d_prime = (d * cos_t) + (e * sin_t) - e_prime = (-(d * sin_t)) + (e * cos_t) - f_prime = f - - x_prime = (-d_prime) / (2 * a_prime) - y_prime = (-e_prime) / (2 * c_prime) - x = (x_prime * cos_t) - (y_prime * sin_t) - y = (x_prime * sin_t) + (y_prime * cos_t) - - maj_axis = np.sqrt(((-4 * f_prime * a_prime * c_prime) + - (c_prime * (d_prime ** 2)) + - (a_prime * (e_prime ** 2))) / (4 * a_prime * (c_prime ** 2))) - min_axis = np.sqrt(((-4 * f_prime * a_prime * c_prime) + - (c_prime * (d_prime ** 2)) + - (a_prime * (e_prime ** 2))) / (4 * (a_prime ** 2) * c_prime)) - - if a_prime > c_prime: - x_axis = min_axis - y_axis = maj_axis - else: - x_axis = maj_axis - y_axis = min_axis - - general_coeffs = (x, y), (x_axis, y_axis), angle - - if DEBUG: - print("Cos: {0}, Sin: {1}".format(cos_t, sin_t)) - print("Conic form = {0:.4f}x^2 + {1:.4f}xy + " - "{2:.4f}y^2 + {3:.4f}x + {4:.4f}y + {5:.4f}".format(*conic_coeffs)) - print("Conic form 2 = {0:.4f}x^2 + {1:.4f}xy + " - "{2:.4f}y^2 + {3:.4f}x + {4:.4f}y + {5:.4f}".format(a_prime, 0, c_prime, - d_prime, e_prime, f_prime)) - print("Elliptical form: Center = {0}, Radii = {1}, Angle = {2}\n".format(*general_coeffs)) - - return general_coeffs - - -def _general_to_rotated_int(conic_coeffs, return_float=False): - """Transform from conic section format to general format, in integer precision. - - :param conic_coeffs: The six coefficients defining the ellipse as a conic shape in - :math:`ax^2 + bxy + cy^2 + dx + ey + f = 0`. - :type conic_coeffs: :py:class:`numpy.ndarray` or tuple - :return: The general form for the ellipse. Returns tuple :math:`(x_c,\ y_c),\\ (a,\\ b),\\ \\theta` - that fulfills the equation - - .. math:: - - \\frac{((x-x_c)\\cos(\\theta) + (y-y_c)\\sin(\\theta))^2}{a^2} + - \\frac{((x-x_c)\\sin(\\theta) - (y-y_c)\\sin(\\theta))^2}{b^2} = 1 - - :rtype: tuple - - """ - # These conic coefficients have a scaling e_norm = sqrt(a**2 + b**2 + c**2) from - # the eigenvector not being normalized before turning it into conic coefficients. - a, b, c, d, e, f = conic_coeffs - angle = np.arctan2(b, (a - c)) / 2 - - if b == 0: - if (a-c) < 0: - unity = 1 - cos_t = 0 - sin_t = 1 - else: - unity = 1 - cos_t = 1 - sin_t = 0 - elif (a-c) == 0: - if b < 0: - unity = 50 - cos_t = 5 - sin_t = -5 - else: - unity = 50 - cos_t = 5 - sin_t = 5 - else: - # unity here becomes a value with scaling e_norm. - cos_2t, sin_2t, unity = ma.Givens_rotation_int(b, (a-c)) - - cos_2t, sin_2t = -sin_2t, cos_2t - - if cos_2t < 0: - sin_t = ma.sqrt_int64(int((unity - cos_2t) >> 1), False) # sin_t is scaled up sqrt(unity) - cos_t = sin_2t // (2 * sin_t) # cos_t also becomes sqrt(unity) scaled - # Now that we have cos and sin values we establish a new scaling with - # unity value obtained through the sin^2(x) + cos^2(x) = 1 trigonometric identity. - unity = sin_t ** 2 + cos_t ** 2 - - else: - cos_t = ma.sqrt_int64(int((cos_2t + unity) >> 1), False) # cos_t is scaled up sqrt(unity) - sin_t = sin_2t // (2 * cos_t) # sin_t also becomes sqrt(unity) scaled - # Now that we have cos and sin values we establish a new scaling with - # unity value obtained through the sin^2(x) + cos^2(x) = 1 trigonometric identity. - unity = sin_t ** 2 + cos_t ** 2 - - a_prime = a * (cos_t ** 2) + b * cos_t * sin_t + c * (sin_t ** 2) # Is unity + e_norm scaled - c_prime = a * (sin_t ** 2) - b * cos_t * sin_t + c * (cos_t ** 2) # Is unity + e_norm scaled - d_prime = (d * cos_t) + (e * sin_t) # Is sqrt(unity) + e_norm scaled - e_prime = (-(d * sin_t)) + (e * cos_t) # Is sqrt(unity) + e_norm scaled - f_prime = f # Is e_norm scaled only. - - x_prime_num = (-d_prime) - x_prime_denom = (2 * a_prime) - - y_prime_num = (-e_prime) - y_prime_denom = (2 * c_prime) - - if return_float: - # At sub-pixel precision, treat values as floats. - x = (((x_prime_num * cos_t) / x_prime_denom) - ((y_prime_num * sin_t) / y_prime_denom)) - y = (((x_prime_num * sin_t) / x_prime_denom) + ((y_prime_num * cos_t) / y_prime_denom)) - else: - # At pixel precision, perform integer division. - x = int(((x_prime_num * cos_t) // x_prime_denom) - - ((y_prime_num * sin_t) // y_prime_denom)) - y = int(((x_prime_num * sin_t) // x_prime_denom) + - ((y_prime_num * cos_t) // y_prime_denom)) - - sqrt_unity = ma.sqrt_int64(int(unity)) - - a_prime = a_prime // (unity) - c_prime = c_prime // (unity) - d_prime = d_prime // (sqrt_unity) - e_prime = e_prime // (sqrt_unity) - - if return_float: - # At sub-pixel precision, use float divison and square root. - numerator = ((-4 * f_prime * a_prime * c_prime) + - (c_prime * (d_prime ** 2)) + - (a_prime * (e_prime ** 2))) - denominator_major = (4 * a_prime * (c_prime ** 2)) - tmp_axis = ma.sqrt_int64((numerator // denominator_major), True) - maj_axis = tmp_axis[0] + tmp_axis[1] - denominator_minor = (4 * (a_prime ** 2) * c_prime) - tmp_axis = ma.sqrt_int64(numerator // denominator_minor, True) - min_axis = tmp_axis[0] + tmp_axis[1] - else: - # At pixel precision, use integer division and perform - # fixed point square root. - numerator = ((-4 * f_prime * a_prime * c_prime) + - (c_prime * (d_prime ** 2)) + - (a_prime * (e_prime ** 2))) * unity - denominator_major = (4 * a_prime * (c_prime ** 2)) - maj_axis = ma.sqrt_int64(int(numerator // denominator_major)) - denominator_minor = (4 * (a_prime ** 2) * c_prime) - min_axis = ma.sqrt_int64(int(numerator // denominator_minor)) - - if a_prime > c_prime: - x_axis = min_axis - y_axis = maj_axis - else: - x_axis = maj_axis - y_axis = min_axis - - general_coeffs = (x, y), (x_axis, y_axis), angle - - if DEBUG: - print("Cos: {0} => {1}, Sin: {2} => {3}".format(cos_t, cos_t / np.sqrt(unity), sin_t, sin_t / np.sqrt(unity))) - print("Conic form = {0:.4f}x^2 + {1:.4f}xy + " - "{2:.4f}y^2 + {3:.4f}x + {4:.4f}y + {5:.4f}".format(*conic_coeffs)) - print("Conic form 2 = {0:.4f}x^2 + {1:.4f}xy + " - "{2:.4f}y^2 + {3:.4f}x + {4:.4f}y + {5:.4f}".format( - a_prime * unity, 0, c_prime * unity, d_prime * np.sqrt(unity), e_prime * np.sqrt(unity), f_prime)) - print("Elliptical form: Center = {0}, Radii = {1}, Angle = {2}\n".format(*general_coeffs)) - - return general_coeffs - - -def _remove_mean_values(points): - x_mean = int(points[:, 0].mean()) - y_mean = int(points[:, 1].mean()) - return points - (x_mean, y_mean), x_mean, y_mean - - -def _calculate_M_and_T_double(points): - """Part of the B2AC ellipse fitting algorithm, calculating the M and T - matrices needed. - - :param points: The [Nx2] array of points to fit ellipse to. - :type points: :py:class:`numpy.ndarray` - :return: Matrices M and T. - :rtype: tuple - - """ - S = _calculate_scatter_matrix_py(points[:, 0], points[:, 1]) - S3 = np.array([S[3, 3], S[3, 4], S[3, 5], S[4, 4], S[4, 5], S[5, 5]]) - S3_inv = mo.inverse_symmetric_3by3_double(S3).reshape((3, 3)) - S2 = S[:3, 3:] - T = -np.dot(S3_inv, S2.T) - M_term_2 = np.dot(S2, T) - M = S[:3, :3] + M_term_2 - M[[0, 2], :] /= 2 - M[1, :] = -M[1, :] - - return M, T - - -def _scale_T_matrix(T_no_det, det_S3): - m, M = np.abs(T_no_det).min(), np.abs(T_no_det).max() - if np.log2(M) <= 32: - scale = 0 - elif np.log2(M) <= 40: - scale = 8 - elif np.log2(M) <= 48: - scale = 16 - elif np.log2(M) <= 56: - scale = 24 - else: - scale = 32 - - det_S3 >>= scale - T_no_det >>= scale - - return T_no_det, det_S3 - - -def _calculate_M_and_T_int64(points): - """Part of the B2AC ellipse fitting algorithm, calculating the M and T - matrices needed. - - This integer implementation also returns the determinant of the - scatter matrix, which hasn't been applied to the matrix T yet. - - M is exact in integer values, but is truncated towards zero compared - to the double version. - - :param points: The [Nx2] array of points to fit ellipse to. - :type points: :py:class:`numpy.ndarray` - :return: M, T undivided by determinant and the determinant. - :rtype: tuple - - """ - S = _calculate_scatter_matrix_c(points[:, 0], points[:, 1]) - # Extract the symmetric parts of the S matrix. - S1 = np.array([S[0, 0], S[0, 1], S[0, 2], S[1, 1], S[1, 2], S[2, 2]], dtype='int64') - S3 = np.array([S[3, 3], S[3, 4], S[3, 5], S[4, 4], S[4, 5], S[5, 5]], dtype='int64') - - adj_S3, det_S3 = mo.inverse_symmetric_3by3_int64(S3) - if np.linalg.norm((np.array(adj_S3, 'float') / det_S3) - np.linalg.norm(np.linalg.inv(S[3:, 3:]))) > 0.1: - print("Norm diff: {0}".format(np.linalg.norm(np.array(adj_S3, 'float') / det_S3) - np.linalg.norm(S[3:,3:]))) - S2 = S[:3, 3:] - - T_no_det = - np.dot(np.array(adj_S3.reshape((3, 3)), 'int64'), np.array(S2.T, 'int64')) - T_no_det, det_S3 = _scale_T_matrix(T_no_det, det_S3) - M_term_2 = np.dot(np.array(S2, 'int64'), T_no_det) // det_S3 - M = mo.matrix_add_symmetric(M_term_2, S1) - M[[0, 2], :] /= 2 - M[1, :] = -M[1, :] - - #T_no_det, scale = eig._scale_matrix(T_no_det) - #det_S3 = det_S3 >> scale - - return M, T_no_det, det_S3 - - -def _calculate_scatter_matrix_py(x, y): - """Calculates the complete scatter matrix for the input coordinates. - - :param x: The x coordinates. - :type x: :py:class:`numpy.ndarray` - :param y: The y coordinates. - :type y: :py:class:`numpy.ndarray` - :return: The complete scatter matrix. - :rtype: :py:class:`numpy.ndarray` - - """ - D = np.ones((len(x), 6), 'int64') - D[:, 0] = x * x - D[:, 1] = x * y - D[:, 2] = y * y - D[:, 3] = x - D[:, 4] = y - - return np.dot(D.T, D) - - -def _calculate_scatter_matrix_c(x, y): - """Calculates the upper triangular scatter matrix for the input coordinates. - - :param x: The x coordinates. - :type x: :py:class:`numpy.ndarray` - :param y: The y coordinates. - :type y: :py:class:`numpy.ndarray` - :return: The upper triangular scatter matrix. - :rtype: :py:class:`numpy.ndarray` - - """ - S = np.zeros((6, 6), 'int64') - - for i in xrange(len(x)): - tmp_x2 = x[i] ** 2 - tmp_x3 = tmp_x2 * x[i] - tmp_y2 = y[i] ** 2 - tmp_y3 = tmp_y2 * y[i] - - S[0, 0] += tmp_x2 * tmp_x2 - S[0, 1] += tmp_x3 * y[i] - S[0, 2] += tmp_x2 * tmp_y2 - S[0, 3] += tmp_x3 - S[0, 4] += tmp_x2 * y[i] - S[0, 5] += tmp_x2 - S[1, 2] += tmp_y3 * x[i] - S[1, 4] += tmp_y2 * x[i] - S[1, 5] += x[i] * y[i] - S[2, 2] += tmp_y2 * tmp_y2 - S[2, 4] += tmp_y3 - S[2, 5] += tmp_y2 - S[3, 5] += x[i] - S[4, 5] += y[i] - - S[5, 5] = len(x) - - # Doubles - S[1, 1] = S[0, 2] - S[1, 3] = S[0, 4] - S[2, 3] = S[1, 4] - S[3, 3] = S[0, 5] - S[3, 4] = S[1, 5] - S[4, 4] = S[2, 5] - - return S diff --git a/b2ac/ext/src_2/EllipseFit.cpp b/b2ac/ext/src_2/EllipseFit.cpp new file mode 100644 index 0000000..22cf463 --- /dev/null +++ b/b2ac/ext/src_2/EllipseFit.cpp @@ -0,0 +1,405 @@ +#include "stdafx.h" +#include "EllipseFit.h" + +EllipseDouble FitEllipse_double(int32_t *x, int32_t *y, int16_t numberOfPoints) +{ + static int16_t _i; + // Storage for the scatter matrix. It is a 6x6 symmetrical matrix. + static int32_t *scatterMatrix = (int32_t *) malloc(sizeof(int32_t) * 21); + // Storage for matrices to calculate on the way to eigensystem matrix. + static double *S3Inverse = (double *) malloc(sizeof(double) * 6); + static double *T = (double *) malloc(sizeof(double) * 9); + // Eigensystem matrix storage. + static double *M = (double *) malloc(sizeof(double) * 9); + // Eigenvectors and eigenvalues storage. + //static double *eigenvalues = (double *) malloc(sizeof(double) * 3); + //static double *eigenvectors = (double *) malloc(sizeof(double) * 9); + static double eigenvalues[3] = {1432841.99428,5476.75547089,-1122751.7107}; + static double eigenvectors[9] = {0.889048686155,-0.457588868649,0.00503636728127,-0.0143632566829,-0.00589873101016,-0.999969859516,-0.457587292769,-0.889144325889,-0.00590889697334}; + static double *conicCoefficients = (double *) malloc(sizeof(double) * 6); + + static int32_t _xMean = 0; + static int32_t _yMean = 0; + static int16_t ellipticalEigenvectorIndex = -1; + + static EllipseDouble results; + + // Reset scatter matrix. All other matrices will be replaced "in transit". + for(_i = 0; _i < 21; _i++) + { + scatterMatrix[_i] = 0; + } + + _xMean = removeMeanFromCoordinates(x, numberOfPoints); + _yMean = removeMeanFromCoordinates(y, numberOfPoints); + + // Calculate the T and M matrices needed. + calculateScatterMatrix(x, y, numberOfPoints, scatterMatrix); + matrixInverseSymmetric3by3_double(&scatterMatrix[15], S3Inverse); + calculateTMatrix_double(scatterMatrix, S3Inverse, T); + calculateSecondTermOfM_double(scatterMatrix, T, M); + calculateCompleteM_double(scatterMatrix, M); + + // Now we have M, apply eigensolver to get a1 vector. + printf("Double version:\n"); + for(_i = 0; _i < 9; _i++) + { + printf("M[%d] = %f\n", _i, M[_i]); + } + printf("\n"); + + // Eigenvalues calculated; evaluate which one is elliptical. + // It can be proven that only one eigenvalue will be positive in case + // of elliptical solution; if none fulfills this, then fitting was unsucessful. + ellipticalEigenvectorIndex = -1; + for(_i = 0; _i < 3; _i++) + { + if ((4 * eigenvectors[_i] * eigenvectors[6 + _i]) - (eigenvectors[3 + _i] * eigenvectors[3 + _i]) > 0.0) + { + ellipticalEigenvectorIndex = _i; + break; + } + } + + if (ellipticalEigenvectorIndex == -1) + { + // Non-elliptical solution found. Returning empty results. + results.isValid = false; + } + else + { + // Sort the coefficients calculated and find the three last ones. + conicCoefficients[0] = eigenvectors[_i]; + conicCoefficients[1] = eigenvectors[3 +_i]; + conicCoefficients[2] = eigenvectors[6 + _i]; + conicCoefficients[3] = conicCoefficients[0] * T[0] + conicCoefficients[1] * T[1] + conicCoefficients[2] * T[2]; + conicCoefficients[4] = conicCoefficients[0] * T[3] + conicCoefficients[1] * T[4] + conicCoefficients[2] * T[5]; + conicCoefficients[5] = conicCoefficients[0] * T[6] + conicCoefficients[1] * T[7] + conicCoefficients[2] * T[8]; + + printf("Double version:\n"); + for(_i = 0; _i < 6; _i++) + { + printf("c[%d] = %f\n", _i, conicCoefficients[_i]); + } + printf("\n"); + + // Convert to general form and store this information + // in the output struct. + conicToGeneralForm_double(conicCoefficients, &results); + // Add the previously subtracted mean point. + results.centerX += _xMean; + results.centerY += _yMean; + } + + printf("Double version:\n"); + printf("Center = %f, %f\n", results.centerX, results.centerY); + printf("Axes (Maj, Min) = %f, %f\n", results.majorAxis, results.minorAxis); + printf("Rotation Angle = %f\n", results.rotationAngle); + printf("Valid = %c\n", results.isValid); + + return results; +} + +EllipseDouble FitEllipse_int(int32_t *x, int32_t *y, int16_t numberOfPoints) +{ + static int16_t _i; + // Storage for the scatter matrix. It is a 6x6 symmetrical matrix. + static int32_t *scatterMatrix = (int32_t *) malloc(sizeof(int32_t) * 21); + // Storage for variables and matrices to calculate on the way to eigensystem matrix. + static int64_t *S3Inverse = (int64_t *) malloc(sizeof(int64_t) * 6); + static int64_t S3Determinant = 0; + static int64_t *T = (int64_t *) malloc(sizeof(int64_t) * 9); + // Eigensystem matrix storage. + static int32_t *M = (int32_t *) malloc(sizeof(int32_t) * 9); + // Eigenvectors and eigenvalues storage. + + static int32_t _xMean = 0; + static int32_t _yMean = 0; + + static EllipseDouble results; + + // Reset scatter matrix. All other matrices will be replaced "in transit". + for(_i = 0; _i < 21; _i++) + { + scatterMatrix[_i] = 0; + } + + _xMean = removeMeanFromCoordinates(x, numberOfPoints); + _yMean = removeMeanFromCoordinates(y, numberOfPoints); + + // Calculate the T and M matrices needed. + calculateScatterMatrix(x, y, numberOfPoints, scatterMatrix); + S3Determinant = matrixInverseSymmetric3by3_int(&scatterMatrix[15], S3Inverse); + calculateTMatrix_int(scatterMatrix, S3Inverse, T); + calculateSecondTermOfM_int(scatterMatrix, T, M, S3Determinant); + calculateCompleteM_int(scatterMatrix, M); + + // Now we have M, apply eigensolver to get a1 vector. + printf("Integer version:\n"); + for(_i = 0; _i < 9; _i++) + { + printf("M[%d] = %d\n", _i, M[_i]); + } + printf("\n"); + + return results; +} + +int32_t removeMeanFromCoordinates(int32_t *coords, int16_t numberOfPoints) +{ + return 0; + static int16_t _i; + static int32_t _meanPoint; + + _meanPoint = 0; + for(_i = 0; _i < numberOfPoints; _i++) + { + _meanPoint += coords[_i]; + } + + _meanPoint /= numberOfPoints; + + for(_i = 0; _i < numberOfPoints; _i++) + { + coords[_i] -= _meanPoint; + } + + return (_meanPoint); +} + +void calculateScatterMatrix(int32_t *x, int32_t *y, int16_t numberOfPoints, int32_t *scatterMatrix) +{ + static int16_t _i; + static int32_t tmpX2; + static int32_t tmpX3; + static int32_t tmpY2; + static int32_t tmpY3; + + for(_i = 0; _i < numberOfPoints; _i++) + { + tmpX2 = x[_i] * x[_i]; + tmpX3 = tmpX2 * x[_i]; + tmpY2 = y[_i] * y[_i]; + tmpY3 = tmpY2 * y[_i]; + + // First row of scatter matrix + scatterMatrix[0] += tmpX2 * tmpX2; + scatterMatrix[1] += tmpX3 * y[_i]; + scatterMatrix[2] += tmpX2 * tmpY2; + scatterMatrix[3] += tmpX3; + scatterMatrix[4] += tmpX2 * y[_i]; + scatterMatrix[5] += tmpX2; + + // Second row of scatter matrix + // Index 6 is a duplicate of index 2. + scatterMatrix[7] += tmpY3 * x[_i]; + // Index 8 is a duplicate of index 4. + scatterMatrix[9] += tmpY2 * x[_i]; + scatterMatrix[10] += x[_i] * y[_i]; + + // Third row of scatter matrix + scatterMatrix[11] += tmpY2 * tmpY2; + // Index 12 is a duplicate of index 9. + scatterMatrix[13] += tmpY2 * y[_i]; + scatterMatrix[14] += tmpY2; + + // Fourth row of scatter matrix + // Index 15 is a duplicate of index 5. + // Index 16 is a duplicate of index 10. + scatterMatrix[17] += x[_i]; + + // Fifth row of scatter matrix + // Index 18 is a duplicate of index 14. + scatterMatrix[19] += y[_i]; + } + + // Set duplicates. + scatterMatrix[6] = scatterMatrix[2]; + scatterMatrix[8] = scatterMatrix[4]; + scatterMatrix[12] = scatterMatrix[9]; + scatterMatrix[15] = scatterMatrix[5]; + scatterMatrix[16] = scatterMatrix[10]; + scatterMatrix[18] = scatterMatrix[14]; + + // Set number of points in last slot. + scatterMatrix[20] = (int32_t) numberOfPoints; +} + +void matrixInverseSymmetric3by3_double(int32_t *matrix, double *inverse) +{ + static double determinant; + + // First row of inverse matrix + inverse[0] = (double) (matrix[3] * matrix[5] - (matrix[4] * matrix[4])); + inverse[1] = (double) -(matrix[1] * matrix[5] - matrix[4] * matrix[2]); + inverse[2] = (double) (matrix[1] * matrix[4] - matrix[3] * matrix[2]); + + // Calculate determinant. + determinant = (double) matrix[0] * inverse[0] + matrix[1] * inverse[1] + matrix[2] * inverse[2]; + + // Apply determinant of the first row. + inverse[0] /= determinant; + inverse[1] /= determinant; + inverse[2] /= determinant; + + // Second row of inverse matrix + inverse[3] = ((double) (matrix[0] * matrix[5] - (matrix[2] * matrix[2]))) / determinant; + inverse[4] = ((double) -(matrix[0] * matrix[4] - matrix[1] * matrix[2])) / determinant; + + // Third row of inverse matrix + inverse[5] = ((double) (matrix[0] * matrix[3] - (matrix[1] * matrix[1]))) / determinant; +} + +int64_t matrixInverseSymmetric3by3_int(int32_t *matrix, int64_t *inverse) +{ + // First row of adjoint matrix + inverse[0] = (int64_t) (matrix[3] * matrix[5] - (matrix[4] * matrix[4])); + inverse[1] = (int64_t) -(matrix[1] * matrix[5] - matrix[4] * matrix[2]); + inverse[2] = (int64_t) (matrix[1] * matrix[4] - matrix[3] * matrix[2]); + + // Second row of adjoint matrix + inverse[3] = (int64_t) (matrix[0] * matrix[5] - (matrix[2] * matrix[2])); + inverse[4] = (int64_t) -(matrix[0] * matrix[4] - matrix[1] * matrix[2]); + + // Third row of adjoint matrix + inverse[5] = (int64_t) (matrix[0] * matrix[3] - (matrix[1] * matrix[1])); + + // Return determinant. + return (matrix[0] * inverse[0] + matrix[1] * inverse[1] + matrix[2] * inverse[2]); +} + +void calculateTMatrix_double(int32_t *sm, double *s3i, double *T) +{ + // First row of T matrix + T[0] = -(s3i[0] * sm[3] + s3i[1] * sm[4] + s3i[2] * sm[5]); + T[1] = -(s3i[0] * sm[4] + s3i[1] * sm[9] + s3i[2] * sm[10]); + T[2] = -(s3i[0] * sm[9] + s3i[1] * sm[13] + s3i[2] * sm[14]); + + // Second row of T matrix + T[3] = -(s3i[1] * sm[3] + s3i[3] * sm[4] + s3i[4] * sm[5]); + T[4] = -(s3i[1] * sm[4] + s3i[3] * sm[9] + s3i[4] * sm[10]); + T[5] = -(s3i[1] * sm[9] + s3i[3] * sm[13] + s3i[4] * sm[14]); + + // Third row of T matrix + T[6] = -(s3i[2] * sm[3] + s3i[4] * sm[4] + s3i[5] * sm[5]); + T[7] = -(s3i[2] * sm[4] + s3i[4] * sm[9] + s3i[5] * sm[10]); + T[8] = -(s3i[2] * sm[9] + s3i[4] * sm[13] + s3i[5] * sm[14]); +} + +void calculateTMatrix_int(int32_t *sm, int64_t *s3i, int64_t *T) +{ + // First row of T matrix + T[0] = -(s3i[0] * sm[3] + s3i[1] * sm[4] + s3i[2] * sm[5]); + T[1] = -(s3i[0] * sm[4] + s3i[1] * sm[9] + s3i[2] * sm[10]); + T[2] = -(s3i[0] * sm[9] + s3i[1] * sm[13] + s3i[2] * sm[14]); + + // Second row of T matrix + T[3] = -(s3i[1] * sm[3] + s3i[3] * sm[4] + s3i[4] * sm[5]); + T[4] = -(s3i[1] * sm[4] + s3i[3] * sm[9] + s3i[4] * sm[10]); + T[5] = -(s3i[1] * sm[9] + s3i[3] * sm[13] + s3i[4] * sm[14]); + + // Third row of T matrix + T[6] = -(s3i[2] * sm[3] + s3i[4] * sm[4] + s3i[5] * sm[5]); + T[7] = -(s3i[2] * sm[4] + s3i[4] * sm[9] + s3i[5] * sm[10]); + T[8] = -(s3i[2] * sm[9] + s3i[4] * sm[13] + s3i[5] * sm[14]); +} + +void calculateSecondTermOfM_double(int32_t *sm, double *T, double *M) +{ + // First row of second term of M matrix + M[0] = (T[0] * sm[3] + T[3] * sm[4] + T[6] * sm[5]); + M[1] = (T[1] * sm[3] + T[4] * sm[4] + T[7] * sm[5]); + M[2] = (T[2] * sm[3] + T[5] * sm[4] + T[8] * sm[5]); + + // Second row of second term of M matrix + M[3] = (T[0] * sm[4] + T[3] * sm[9] + T[6] * sm[10]); + M[4] = (T[1] * sm[4] + T[4] * sm[9] + T[7] * sm[10]); + M[5] = (T[2] * sm[4] + T[5] * sm[9] + T[8] * sm[10]); + + // Third row of second term of M matrix + M[6] = (T[0] * sm[9] + T[3] * sm[13] + T[6] * sm[14]); + M[7] = (T[1] * sm[9] + T[4] * sm[13] + T[7] * sm[14]); + M[8] = (T[2] * sm[9] + T[5] * sm[13] + T[8] * sm[14]); +} + +void calculateSecondTermOfM_int(int32_t *sm, int64_t *T, int32_t *M, int64_t determinant) +{ + // First row of second term of M matrix + M[0] = (int32_t) ((T[0] * sm[3] + T[3] * sm[4] + T[6] * sm[5]) / determinant); + M[1] = (int32_t) ((T[1] * sm[3] + T[4] * sm[4] + T[7] * sm[5]) / determinant); + M[2] = (int32_t) ((T[2] * sm[3] + T[5] * sm[4] + T[8] * sm[5]) / determinant); + + // Second row of second term of M matrix + M[3] = (int32_t) ((T[0] * sm[4] + T[3] * sm[9] + T[6] * sm[10]) / determinant); + M[4] = (int32_t) ((T[1] * sm[4] + T[4] * sm[9] + T[7] * sm[10]) / determinant); + M[5] = (int32_t) ((T[2] * sm[4] + T[5] * sm[9] + T[8] * sm[10]) / determinant); + + // Third row of second term of M matrix + M[6] = (int32_t) ((T[0] * sm[9] + T[3] * sm[13] + T[6] * sm[14]) / determinant); + M[7] = (int32_t) ((T[1] * sm[9] + T[4] * sm[13] + T[7] * sm[14]) / determinant); + M[8] = (int32_t) ((T[2] * sm[9] + T[5] * sm[13] + T[8] * sm[14]) / determinant); +} + +void calculateCompleteM_double(int32_t *sm, double *M) +{ + // Add the two matrices S1 and -S2*S3^{-1}*S2^{T} and perform + // left multiplication with C^{-1} simultaneously. + M[0] = (M[0] + sm[0]) / 2; + M[1] = (M[1] + sm[1]) / 2; + M[3] -= sm[1]; + M[2] = (M[2] + sm[2]) / 2; + M[6] = (M[6] + sm[2]) / 2; + M[4] -= sm[6]; + M[5] -= sm[7]; + M[7] = (M[7] + sm[7]) / 2; + M[8] = (M[8] + sm[11]) / 2; +} + +void calculateCompleteM_int(int32_t *sm, int32_t *M) +{ + // Add the two matrices S1 and -S2*S3^{-1}*S2^{T} and perform + // left multiplication with C^{-1} simultaneously. + M[0] = (M[0] + sm[0]) / 2; + M[1] = (M[1] + sm[1]) / 2; + M[3] -= sm[1]; + M[2] = (M[2] + sm[2]) / 2; + M[6] = (M[6] + sm[2]) / 2; + M[4] -= sm[6]; + M[5] -= sm[7]; + M[7] = (M[7] + sm[7]) / 2; + M[8] = (M[8] + sm[11]) / 2; +} + +void conicToGeneralForm_double(double *cc, EllipseDouble *results) +{ + static double centerPointDenominator = 0; + static double mu = 0; + // Storing to avoid recalculation. + static double muA = 0; + static double muB = 0; + static double muC = 0; + static double first_term = 0; + static double sqrt_term = 0; + + centerPointDenominator = 2 * ((cc[1] * cc[1]) - (cc[0] * cc[2])); + results->centerX = ((cc[2] * cc[3]) - (cc[1] * cc[4])) / centerPointDenominator; + results->centerY = ((cc[0] * cc[3]) - (cc[1] * cc[3])) / centerPointDenominator; + + mu = 1 / ((cc[0] * results->centerX * results->centerX) + + (2 * cc[1] * results->centerX * results->centerY) + + (cc[2] * results->centerY * results->centerY) - + cc[5]); + + muA = mu * cc[0]; + muB = mu * cc[1]; + muC = mu * cc[2]; + + sqrt_term = sqrt(((muA - muC) * (muA - muC)) + (4 * muB * muB)) / 2; + first_term = (muA + muC) / 2; + + results->minorAxis = 1 / sqrt(first_term + sqrt_term); + results->majorAxis = 1 / sqrt(first_term - sqrt_term); + + results->rotationAngle = atan2(-2 * cc[1], cc[2] - cc[0]); + results->isValid = true; +} diff --git a/b2ac/ext/src_2/EllipseFit.h b/b2ac/ext/src_2/EllipseFit.h new file mode 100644 index 0000000..ca98614 --- /dev/null +++ b/b2ac/ext/src_2/EllipseFit.h @@ -0,0 +1,42 @@ +#include +#include +#include +#include + +typedef struct _EllipseDouble +{ + double centerX; + double centerY; + double majorAxis; + double minorAxis; + double rotationAngle; + bool isValid; +} EllipseDouble; + +EllipseDouble FitEllipse_double(int32_t *x, int32_t *y, int16_t numberOfPoints); + +EllipseDouble FitEllipse_int(int32_t *x, int32_t *y, int16_t numberOfPoints); + +int32_t removeMeanFromCoordinates(int32_t *coords, int16_t numberOfPoints); + +void calculateScatterMatrix(int32_t *x, int32_t *y, int16_t numberOfPoints, int32_t *scatterMatrix); + +void matrixInverseSymmetric3by3_double(int32_t *matrix, double *inverse); + +int64_t matrixInverseSymmetric3by3_int(int32_t *matrix, int64_t *inverse); + +void calculateTMatrix_double(int32_t *sm, double *s3i, double *T); + +void calculateTMatrix_int(int32_t *sm, int64_t *s3i, int64_t *T); + +void calculateSecondTermOfM_double(int32_t *sm, double *T, double *M); + +void calculateSecondTermOfM_int(int32_t *sm, int64_t *T, int32_t *M, int64_t determinant); + +void calculateCompleteM_double(int32_t *sm, double *M); + +void calculateCompleteM_int(int32_t *sm, int32_t *M); + +void conicToGeneralForm_double(double *conicCoefficients, EllipseDouble *results); + + diff --git a/b2ac/ext/src_2/EllipseFitter.cpp b/b2ac/ext/src_2/EllipseFitter.cpp new file mode 100644 index 0000000..e05ad77 --- /dev/null +++ b/b2ac/ext/src_2/EllipseFitter.cpp @@ -0,0 +1,18 @@ +// EllipseFitter.cpp : Defines the entry point for the console application. +// + +#include "stdafx.h" +#include "EllipseFit.h" + +int _tmain(int argc, _TCHAR* argv[]) +{ + int32_t x[73] = {23,22,22,22,21,21,19,19,17,16,14,13,11,9,7,5,3,2,-1,-1,-4,-6,-7,-9,-11,-12,-15,-16,-16,-18,-19,-20,-20,-21,-21,-23,-22,-22,-22,-21,-21,-20,-19,-18,-17,-15,-14,-12,-11,-10,-8,-5,-4,-2,0,1,4,6,8,10,11,13,13,16,16,18,18,20,21,22,22,22,22}; + int32_t y[73] = {0,1,2,4,5,6,8,9,10,12,13,13,14,14,15,15,15,16,16,16,15,16,15,16,14,13,12,12,11,10,9,7,5,5,2,1,0,-2,-2,-4,-6,-7,-8,-9,-10,-11,-13,-13,-14,-14,-14,-15,-16,-16,-16,-16,-16,-16,-16,-14,-14,-12,-12,-11,-10,-9,-8,-7,-6,-4,-3,-1,0}; + int16_t numberOfPoints = 73; + + FitEllipse_double(x, y, numberOfPoints); + FitEllipse_int(x, y, numberOfPoints); + getchar(); + return 0; +} + diff --git a/b2ac/fit/__init__.py b/b2ac/fit/__init__.py new file mode 100644 index 0000000..95797f7 --- /dev/null +++ b/b2ac/fit/__init__.py @@ -0,0 +1,4 @@ +from .unstable import fit_unstable_B2AC +from .double import fit_improved_B2AC_double +from .reference import fit_improved_B2AC_numpy +from .int import fit_improved_B2AC_int diff --git a/b2ac/fit/double.py b/b2ac/fit/double.py new file mode 100644 index 0000000..466d3c5 --- /dev/null +++ b/b2ac/fit/double.py @@ -0,0 +1,114 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +:mod:`fit` +================== + +.. module:: fit + :synopsis: + +.. moduleauthor:: hbldh + +Created on 2015-09-24, 07:18:22 + +""" + +from __future__ import division +from __future__ import print_function +from __future__ import unicode_literals +from __future__ import absolute_import + +import numpy as np + +import b2ac.matrix.matrix_operations as mo +import b2ac.eigenmethods.qr_algorithm as qr +import b2ac.eigenmethods.inverse_iteration as inv_iter + + +def fit_improved_B2AC_double(points): + """Ellipse fitting in Python with improved B2AC algorithm as described in + this `paper `_. + + This version of the fitting uses float storage during calculations and performs the + eigensolver on a float array. It only uses `b2ac` package methods for fitting, to + be as similar to the integer implementation as possible. + + :param points: The [Nx2] array of points to fit ellipse to. + :type points: :py:class:`numpy.ndarray` + :return: The conic section array defining the fitted ellipse. + :rtype: :py:class:`numpy.ndarray` + + """ + e_conds = [] + points = np.array(points, 'float') + + M, T = _calculate_M_and_T_double(points) + + e_vals = sorted(qr.QR_algorithm_shift_Givens_double(M)[0]) + + a = None + for ev_ind in [1, 2, 0]: + # Find the eigenvector that matches this eigenvector. + eigenvector = inv_iter.inverse_iteration_for_eigenvector_double(M, e_vals[ev_ind], 5) + + # See if that eigenvector yields an elliptical solution. + elliptical_condition = (4 * eigenvector[0] * eigenvector[2]) - (eigenvector[1] ** 2) + e_conds.append(elliptical_condition) + if elliptical_condition > 0: + a = eigenvector + break + + if a is None: + print("Eigenvalues = {0}".format(e_vals)) + print("Elliptical conditions = {0}".format(e_conds)) + raise ArithmeticError("No elliptical solution found.") + + conic_coefficients = np.concatenate((a, np.dot(T, a))) + return conic_coefficients + + +def _calculate_M_and_T_double(points): + """Part of the B2AC ellipse fitting algorithm, calculating the M and T + matrices needed. + + :param points: The [Nx2] array of points to fit ellipse to. + :type points: :py:class:`numpy.ndarray` + :return: Matrices M and T. + :rtype: tuple + + """ + S = _calculate_scatter_matrix_double(points[:, 0], points[:, 1]) + S1 = S[:3, :3] + S3 = np.array([S[3, 3], S[3, 4], S[3, 5], S[4, 4], S[4, 5], S[5, 5]]) + S3_inv = mo.inverse_symmetric_3by3_double(S3).reshape((3, 3)) + S2 = S[:3, 3:] + T = -np.dot(S3_inv, S2.T) + M_term_2 = np.dot(S2, T) + M = S1 + M_term_2 + M[[0, 2], :] = M[[2, 0], :] / 2 + M[1, :] = -M[1, :] + + return M, T + + +def _calculate_scatter_matrix_double(x, y): + """Calculates the complete scatter matrix for the input coordinates. + + :param x: The x coordinates. + :type x: :py:class:`numpy.ndarray` + :param y: The y coordinates. + :type y: :py:class:`numpy.ndarray` + :return: The complete scatter matrix. + :rtype: :py:class:`numpy.ndarray` + + """ + D = np.ones((len(x), 6), 'int64') + D[:, 0] = x * x + D[:, 1] = x * y + D[:, 2] = y * y + D[:, 3] = x + D[:, 4] = y + + return D.T.dot(D) + + diff --git a/b2ac/fit/int.py b/b2ac/fit/int.py new file mode 100644 index 0000000..d9185e7 --- /dev/null +++ b/b2ac/fit/int.py @@ -0,0 +1,146 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +:mod:`fit` +================== + +.. module:: fit + :platform: Unix, Windows + :synopsis: + +.. moduleauthor:: hbldh + +Created on 2015-10-13 + +""" + +from __future__ import division +from __future__ import print_function +from __future__ import unicode_literals +from __future__ import absolute_import + +import numpy as np + +from b2ac.eigenmethods.qr_algorithm import QR_algorithm_shift_Givens_int +from b2ac.eigenmethods.inverse_iteration import inverse_iteration_for_eigenvector_int +from b2ac.matrix.matrix_operations import inverse_symmetric_3by3_int64, matrix_add_symmetric +from b2ac.matrix.fixed_point import scale_T_matrix + +__all__ = ['fit_improved_B2AC_int'] + + +def fit_improved_B2AC_int(points): + """Ellipse fitting in Python with improved B2AC algorithm as described in + this `paper `_. + + This version of the fitting uses int64 storage during calculations and performs the + eigensolver on an integer array. + + :param points: The [Nx2] array of points to fit ellipse to. + :type points: :py:class:`numpy.ndarray` + :return: The conic section coefficients array defining the fitted ellipse. + :rtype: :py:class:`numpy.ndarray` + + """ + e_conds = [] + M, T_no_det, determinant_S3 = _calculate_M_and_T_int64(points) + + e_vals = sorted(QR_algorithm_shift_Givens_int(M)[0]) + + a = None + for ev_ind in [1, 2, 0]: + # Find the eigenvector that matches this eigenvector. + eigenvector, e_norm = inverse_iteration_for_eigenvector_int(M, e_vals[ev_ind]) + # See if that eigenvector yields an elliptical solution. + elliptical_condition = (4 * eigenvector[0] * eigenvector[2]) - (eigenvector[1] ** 2) + e_conds.append(elliptical_condition) + if elliptical_condition > 0: + a = eigenvector + break + + if a is None: + raise ArithmeticError("No elliptical solution found.") + + conic_coefficients = np.concatenate((a, np.dot(T_no_det, a) // determinant_S3)) + return conic_coefficients + + +def _calculate_M_and_T_int64(points): + """Part of the B2AC ellipse fitting algorithm, calculating the M and T + matrices needed. + + This integer implementation also returns the determinant of the + scatter matrix, which hasn't been applied to the matrix T yet. + + M is exact in integer values, but is truncated towards zero compared + to the double version. + + :param points: The [Nx2] array of points to fit ellipse to. + :type points: :py:class:`numpy.ndarray` + :return: M, T undivided by determinant and the determinant. + :rtype: tuple + + """ + S = _calculate_scatter_matrix_c(points[:, 0], points[:, 1]) + # Extract the symmetric parts of the S matrix. + S1 = np.array([S[0, 0], S[0, 1], S[0, 2], S[1, 1], S[1, 2], S[2, 2]], dtype='int64') + S3 = np.array([S[3, 3], S[3, 4], S[3, 5], S[4, 4], S[4, 5], S[5, 5]], dtype='int64') + + adj_S3, det_S3 = inverse_symmetric_3by3_int64(S3) + S2 = S[:3, 3:] + + T_no_det = - np.dot(np.array(adj_S3.reshape((3, 3)), 'int64'), np.array(S2.T, 'int64')) + T_no_det, det_S3 = scale_T_matrix(T_no_det, det_S3) + M_term_2 = np.dot(np.array(S2, 'int64'), T_no_det) // det_S3 + M = matrix_add_symmetric(M_term_2, S1) + M[[0, 2], :] //= 2 + M[1, :] = -M[1, :] + + return M, T_no_det, det_S3 + + +def _calculate_scatter_matrix_c(x, y): + """Calculates the upper triangular scatter matrix for the input coordinates. + + :param x: The x coordinates. + :type x: :py:class:`numpy.ndarray` + :param y: The y coordinates. + :type y: :py:class:`numpy.ndarray` + :return: The upper triangular scatter matrix. + :rtype: :py:class:`numpy.ndarray` + + """ + S = np.zeros((6, 6), 'int64') + + for i in xrange(len(x)): + tmp_x2 = x[i] ** 2 + tmp_x3 = tmp_x2 * x[i] + tmp_y2 = y[i] ** 2 + tmp_y3 = tmp_y2 * y[i] + + S[0, 0] += tmp_x2 * tmp_x2 + S[0, 1] += tmp_x3 * y[i] + S[0, 2] += tmp_x2 * tmp_y2 + S[0, 3] += tmp_x3 + S[0, 4] += tmp_x2 * y[i] + S[0, 5] += tmp_x2 + S[1, 2] += tmp_y3 * x[i] + S[1, 4] += tmp_y2 * x[i] + S[1, 5] += x[i] * y[i] + S[2, 2] += tmp_y2 * tmp_y2 + S[2, 4] += tmp_y3 + S[2, 5] += tmp_y2 + S[3, 5] += x[i] + S[4, 5] += y[i] + + S[5, 5] = len(x) + + # Doubles + S[1, 1] = S[0, 2] + S[1, 3] = S[0, 4] + S[2, 3] = S[1, 4] + S[3, 3] = S[0, 5] + S[3, 4] = S[1, 5] + S[4, 4] = S[2, 5] + + return S diff --git a/b2ac/fit/reference.py b/b2ac/fit/reference.py new file mode 100644 index 0000000..f036f47 --- /dev/null +++ b/b2ac/fit/reference.py @@ -0,0 +1,238 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +.. module:: ellipse_fitting + :platform: Unix, Windows + :synopsis: Ellipse fitting algorithms and handling of ellipse information. + +.. moduleauthor:: hbldh + +Created on 2013-05-05, 23:22 + +""" + +from __future__ import division +from __future__ import print_function +from __future__ import unicode_literals +from __future__ import absolute_import + +import numpy as np +from b2ac.matrix.matrix_ref import inverse_symmetric_3by3_double, inverse_symmetric_3by3_int, add_symmetric_matrix + + +def fit_B2AC(points): + """Ellipse fitting in Python with numerically unstable algorithm. + + Described `here `_. + + N_POLYPOINTS.B. Do not use, since it works with almost singular matrix. + + :param points: The [Nx2] array of points to fit ellipse to. + :type points: :py:class:`numpy.ndarray` + :return: The conic section array defining the fitted ellipse. + :rtype: :py:class:`numpy.ndarray` + + """ + import scipy.linalg as scla + + constraint_matrix = np.zeros((6, 6)) + constraint_matrix[0, 2] = 2 + constraint_matrix[1, 1] = -1 + constraint_matrix[2, 0] = 2 + + S = _calculate_scatter_matrix_py(points[:, 0], points[:, 1]) + + evals, evect = scla.eig(S, constraint_matrix) + ind = np.where(evals == (evals[evals > 0].min()))[0][0] + return evect[:, ind] + + +def fit_improved_B2AC_numpy(points): + """Ellipse fitting in Python with improved B2AC algorithm as described in + this `paper `_. + + This version of the fitting simply applies NumPy:s methods for calculating + the conic section, modelled after the Matlab code in the paper: + + .. code-block:: + + function a = fit_ellipse(x, y) + + D1 = [x .ˆ 2, x .* y, y .ˆ 2]; % quadratic part of the design matrix + D2 = [x, y, ones(size(x))]; % linear part of the design matrix + S1 = D1’ * D1; % quadratic part of the scatter matrix + S2 = D1’ * D2; % combined part of the scatter matrix + S3 = D2’ * D2; % linear part of the scatter matrix + T = - inv(S3) * S2’; % for getting a2 from a1 + M = S1 + S2 * T; % reduced scatter matrix + M = [M(3, :) ./ 2; - M(2, :); M(1, :) ./ 2]; % premultiply by inv(C1) + [evec, eval] = eig(M); % solve eigensystem + cond = 4 * evec(1, :) .* evec(3, :) - evec(2, :) .ˆ 2; % evaluate a’Ca + a1 = evec(:, find(cond > 0)); % eigenvector for min. pos. eigenvalue + a = [a1; T * a1]; % ellipse coefficients + + :param points: The [Nx2] array of points to fit ellipse to. + :type points: :py:class:`numpy.ndarray` + :return: The conic section array defining the fitted ellipse. + :rtype: :py:class:`numpy.ndarray` + + """ + x = points[:, 0] + y = points[:, 1] + + D1 = np.vstack([x ** 2, x * y, y ** 2]).T + D2 = np.vstack([x, y, np.ones((len(x), ), dtype=x.dtype)]).T + S1 = D1.T.dot(D1) + S2 = D1.T.dot(D2) + S3 = D2.T.dot(D2) + T = -np.linalg.inv(S3).dot(S2.T) + M = S1 + S2.dot(T) + M = np.array([M[2, :] / 2, -M[1, :], M[0, :] / 2]) + eval, evec = np.linalg.eig(M) + cond = (4 * evec[:, 0] * evec[:, 2]) - (evec[:, 1] ** 2) + I = np.where(cond > 0)[0] + a1 = evec[:, I[np.argmin(cond[I])]] + return np.concatenate([a1, T.dot(a1)]) + + +def fit_improved_B2AC(points): + """Ellipse fitting in Python with improved B2AC algorithm as described in + this `paper `_. + + This version of the fitting uses float storage during calculations and performs the + eigensolver on a float array. + + :param points: The [Nx2] array of points to fit ellipse to. + :type points: :py:class:`numpy.ndarray` + :return: The conic section array defining the fitted ellipse. + :rtype: :py:class:`numpy.ndarray` + + """ + points = np.array(points, 'float') + S = _calculate_scatter_matrix_py(points[:, 0], points[:, 1]) + S3 = S[3:, 3:] + S3 = np.array([S3[0, 0], S3[0, 1], S3[0, 2], S3[1, 1], S3[1, 2], S3[2, 2]]) + S3_inv = inverse_symmetric_3by3_double(S3).reshape((3, 3)) + S2 = S[:3, 3:] + T = -np.dot(S3_inv, S2.T) + M = S[:3, :3] + np.dot(S2, T) + inv_mat = np.array([[0, 0, 0.5], [0, -1, 0], [0.5, 0, 0]], 'float') + M = inv_mat.dot(M) + + e_vals, e_vect = np.linalg.eig(M) + + try: + elliptical_solution_index = np.where(((4 * e_vect[0, :] * e_vect[2, :]) - ((e_vect[1, :] ** 2))) > 0)[0][0] + except: + # No positive eigenvalues. Fit was not ellipse. + raise ArithmeticError("No elliptical solution found.") + + a = e_vect[:, elliptical_solution_index] + if a[0] < 0: + a = -a + return np.concatenate((a, np.dot(T, a))) + + +def fit_improved_B2AC_int(points): + """Ellipse fitting in Python with improved B2AC algorithm as described in + this `paper `_. + + This version of the fitting uses int64 storage during calculations and performs the + eigensolver on an integer array. + + :param points: The [Nx2] array of points to fit ellipse to. + :type points: :py:class:`numpy.ndarray` + :return: The conic section array defining the fitted ellipse. + :rtype: :py:class:`numpy.ndarray` + + """ + S = _calculate_scatter_matrix_c(points[:, 0], points[:, 1]) + S1 = np.array([S[0, 0], S[0, 1], S[0, 2], S[1, 1], S[1, 2], S[2, 2]]) + S3 = np.array([S[3, 3], S[3, 4], S[3, 5], S[4, 4], S[4, 5], S[5, 5]]) + adj_S3, det_S3 = inverse_symmetric_3by3_int(S3) + S2 = S[:3, 3:] + T_no_det = - np.dot(np.array(adj_S3.reshape((3, 3)), 'int64'), np.array(S2.T, 'int64')) + M_term2 = np.dot(np.array(S2, 'int64'), T_no_det) // det_S3 + M = add_symmetric_matrix(M_term2, S1) + M[[0, 2], :] /= 2 + M[1, :] = -M[1, :] + + e_vals, e_vect = np.linalg.eig(M) + + try: + elliptical_solution_index = np.where(((4 * e_vect[0, :] * e_vect[2, :]) - ((e_vect[1, :] ** 2))) > 0)[0][0] + except: + # No positive eigenvalues. Fit was not ellipse. + raise ArithmeticError("No elliptical solution found.") + a = e_vect[:, elliptical_solution_index] + return np.concatenate((a, np.dot(T_no_det, a) / det_S3)) + + +def _calculate_scatter_matrix_py(x, y): + """Calculates the complete scatter matrix for the input coordinates. + + :param x: The x coordinates. + :type x: :py:class:`numpy.ndarray` + :param y: The y coordinates. + :type y: :py:class:`numpy.ndarray` + :return: The complete scatter matrix. + :rtype: :py:class:`numpy.ndarray` + + """ + D = np.ones((len(x), 6), dtype=x.dtype) + D[:, 0] = x * x + D[:, 1] = x * y + D[:, 2] = y * y + D[:, 3] = x + D[:, 4] = y + + return D.T.dot(D) + + +def _calculate_scatter_matrix_c(x, y): + """Calculates the upper triangular scatter matrix for the input coordinates. + + :param x: The x coordinates. + :type x: :py:class:`numpy.ndarray` + :param y: The y coordinates. + :type y: :py:class:`numpy.ndarray` + :return: The upper triangular scatter matrix. + :rtype: :py:class:`numpy.ndarray` + + """ + S = np.zeros((6, 6), 'int32') + + for i in xrange(len(x)): + tmp_x2 = x[i] ** 2 + tmp_x3 = tmp_x2 * x[i] + tmp_y2 = y[i] ** 2 + tmp_y3 = tmp_y2 * y[i] + + S[0, 0] += tmp_x2 * tmp_x2 + S[0, 1] += tmp_x3 * y[i] + S[0, 2] += tmp_x2 * tmp_y2 + S[0, 3] += tmp_x3 + S[0, 4] += tmp_x2 * y[i] + S[0, 5] += tmp_x2 + S[1, 2] += tmp_y3 * x[i] + S[1, 4] += tmp_y2 * x[i] + S[1, 5] += x[i] * y[i] + S[2, 2] += tmp_y2 * tmp_y2 + S[2, 4] += tmp_y3 + S[2, 5] += tmp_y2 + S[3, 5] += x[i] + S[4, 5] += y[i] + + S[5, 5] = len(x) + + # Doubles + S[1, 1] = S[0, 2] + S[1, 3] = S[0, 4] + S[2, 3] = S[1, 4] + S[3, 3] = S[0, 5] + S[3, 4] = S[1, 5] + S[4, 4] = S[2, 5] + + return S + + diff --git a/b2ac/fit/unstable.py b/b2ac/fit/unstable.py new file mode 100644 index 0000000..8d09ed2 --- /dev/null +++ b/b2ac/fit/unstable.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +:mod:`unstable` +================== + +.. module:: unstable + :platform: Unix, Windows + :synopsis: + +.. moduleauthor:: hbldh + +Created on 2015-10-13 + +""" + +from __future__ import division +from __future__ import print_function +from __future__ import unicode_literals +from __future__ import absolute_import + +import numpy as np +from b2ac.fit.double import _calculate_scatter_matrix_double + + +def fit_unstable_B2AC(points): + """Ellipse fitting in Python with numerically unstable algorithm. Requires SciPy to run! + + Described `here `_. + + N.B. Do not use, since it works with almost singular matrix. + + :param points: The [Nx2] array of points to fit ellipse to. + :type points: :py:class:`numpy.ndarray` + :return: The conic section array defining the fitted ellipse. + :rtype: :py:class:`numpy.ndarray` + + """ + import scipy.linalg as scla + + constraint_matrix = np.zeros((6, 6)) + constraint_matrix[0, 2] = 2 + constraint_matrix[1, 1] = -1 + constraint_matrix[2, 0] = 2 + + S = _calculate_scatter_matrix_double(points[:, 0], points[:, 1]) + + eigenvalues, eigenvalues = scla.eig(S, constraint_matrix) + ind = np.where(eigenvalues == (eigenvalues[eigenvalues > 0].min()))[0][0] + return eigenvalues[:, ind] diff --git a/b2ac/geometry/__init__.py b/b2ac/geometry/__init__.py new file mode 100644 index 0000000..a5682fb --- /dev/null +++ b/b2ac/geometry/__init__.py @@ -0,0 +1,2 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- \ No newline at end of file diff --git a/b2ac/geometry/distance/__init__.py b/b2ac/geometry/distance/__init__.py new file mode 100644 index 0000000..a5682fb --- /dev/null +++ b/b2ac/geometry/distance/__init__.py @@ -0,0 +1,2 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- \ No newline at end of file diff --git a/b2ac/geometry/distance/distance_functions.py b/b2ac/geometry/distance/distance_functions.py new file mode 100644 index 0000000..e342dd9 --- /dev/null +++ b/b2ac/geometry/distance/distance_functions.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +:mod:`distance_functions` +================== + +.. module:: distance_functions + :platform: Unix, Windows + :synopsis: + +.. moduleauthor:: hbldh + +Created on 2015-09-28, 23:25 + +""" + +from __future__ import division +from __future__ import print_function +from __future__ import unicode_literals +from __future__ import absolute_import + +import numpy as np + +from b2ac.geometry.point import B2ACPoint +from b2ac.geometry.ellipse import B2ACEllipse +from b2ac.geometry.polygon import B2ACPolygon + + +def distance(this, other): + if isinstance(this, B2ACPoint): + if isinstance(other, B2ACPoint): + # Distance defined as distance between the Point objects. + return np.linalg.norm(this.point - other.point) + elif isinstance(other, B2ACEllipse): + # Distance defined as distance from this point to center point of ellipse. + return np.linalg.norm(this.point - other.center_point) + elif isinstance(other, B2ACPolygon): + # Distance defined as distance from this point to center point of polygon. + return np.linalg.norm(this.point - np.mean(other.polygon_points, 0)) + # TODO: Better to evaluate distance from any polygon node point? + else: + raise ValueError("Cannot compare B2ACPoint to {0}.".format(type(other))) + elif isinstance(this, B2ACEllipse): + if isinstance(other, B2ACPoint): + # Distance defined as distance from this point to center point of ellipse. + return np.linalg.norm(this.center_point - other.point) + elif isinstance(other, B2ACEllipse): + # Distance defined as distance between center points of ellipses. + return np.linalg.norm(this.center_point - other.center_point) + elif isinstance(other, B2ACPolygon): + # Distance defined as distance from ellipse center point and center of polygon. + return np.linalg.norm(this.center_point - np.mean(other.polygon_points, 0)) + # return np.min(np.sqrt(((other.polygon_points - this.center_point) ** 2).sum(1))) + else: + raise ValueError("Cannot compare B2ACEllipse to {0}.".format(type(other))) + elif isinstance(this, B2ACPolygon): + if isinstance(other, B2ACPoint): + # Distance defined as distance from the other point to center point of polygon. + return np.linalg.norm(other.point - np.mean(this.polygon_points, 0)) + + # TODO: Redefine to minimal distance from center point or any polygon node? + # p1 = np.min(np.sqrt(((this.polygon_points - other.point) ** 2).sum(1))) + # p2 = np.linalg.norm(np.mean(this.polygon_points, 0) - other.point) + # return np.min([p1, p2]) + + elif isinstance(other, B2ACEllipse): + # Distance defined as distance from the center point of the other ellipse + # to center point of polygon. + return np.linalg.norm(other.center_point - np.mean(this.polygon_points, 0)) + + # TODO: Redefine to minimal distance from center point or any polygon node? + # p1 = np.min(np.sqrt(((this.polygon_points - other.center_point) ** 2).sum(1))) + # p2 = np.linalg.norm(np.mean(this.polygon_points, 0) - other.center_point) + # return np.min([p1, p2]) + + elif isinstance(other, B2ACPolygon): + # Distance defined as distances between center points of polygons. + return np.linalg.norm(np.mean(this.polygon_points, 0) - np.mean(other.polygon_points, 0)) + + # TODO: Redefine to minimal distance from center points or any polygon nodes? + # p1 = np.min([np.min(np.sqrt(((v - other.polygon_points) ** 2).sum(1))) for v in this.polygon_points]) + # p2 = np.min(np.sqrt(((this.polygon_points - np.mean(other.polygon_points, 0)) ** 2).sum(1))) + # p3 = np.min(np.sqrt(((other.polygon_points - np.mean(this.polygon_points, 0)) ** 2).sum(1))) + # return np.min([p1, p2, p3]) + else: + raise ValueError("Cannot compare B2ACPolygon to {0}.".format(type(other))) + else: + raise ValueError("Cannot call this method with a {0}.".format(type(this))) diff --git a/b2ac/geometry/ellipse.py b/b2ac/geometry/ellipse.py new file mode 100644 index 0000000..4c4b91e --- /dev/null +++ b/b2ac/geometry/ellipse.py @@ -0,0 +1,92 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +.. module:: ellipse + :platform: Unix, Windows + :synopsis: Ellipse class. + +.. moduleauthor:: hbldh + +Created on 2013-09-09, 16:32 + +""" + +from __future__ import division +from __future__ import print_function +from __future__ import unicode_literals +from __future__ import absolute_import + +import numpy as np + +from b2ac.geometry.shape import B2ACGeometricShape + + +class B2ACEllipse(B2ACGeometricShape): + """An class for representing elliptical shapes.""" + + def __init__(self, center, radii, rotation_angle): + """Constructor for B2ACEllipse""" + + super(B2ACEllipse, self).__init__() + self.center_point = np.array(center, 'float') + self.radii = np.array(radii, 'float') + self.rotation_angle = float(rotation_angle) + + def __str__(self): + return "Ellipse: Center = ({0:.2f}, {1:.2f}), Radii = ({2:.2f}, {3:.2f}), Angle = {4:.4f}".format( + self.center_point[0], self.center_point[1], self.radii[0], self.radii[1], self.rotation_angle) + + def __repr__(self): + return str(self) + + @property + def mpl_patch_arguments(self): + """Returns Matplotlib patch arguments. Can then be plotted by with following + snippet: + + .. code-block:: python + + import matplotlib.pyplot as plt + from matplotlib.patches import Ellipse + from b2ac.ellipse import B2ACEllipse + + e = B2ACEllipse((3.0, 5.0), (2.0, 6.0, 1.57)) + plt.gca().add_patch(Ellipse( + e.mpl_patch_arguments, fill=False, edgecolor='g')) + plt.show() + + """ + return self.center_point, self.radii[0] * 2, self.radii[1] * 2, np.rad2deg(self.rotation_angle) + + def get_center_point(self): + """Returns a center of weight for the object.""" + return self.center_point + + def get_area(self): + """Returns the area covered of the B2ACEllipse.""" + return np.pi * self.radii[0] * self.radii[1] + + def polygonize(self, n=73): + """Gets a approximate polygon array representing the ellipse. + + Note that the last point is the same as the first point, creating a closed + polygon. + + :param n: The number of points to generate. Default is 73 (one vertex every 5 degrees). + :type n: int + :return: An [n x 2] numpy array, describing the boundary vertices of + the polygonized ellipse. + :rtype: :py:class:`numpy.ndarray` + + """ + t = np.linspace(0, 2 * np.pi, num=n, endpoint=True) + out = np.zeros((len(t), 2), dtype='float') + out[:, 0] = (self.center_point[0] + + self.radii[0] * np.cos(t) * np.cos(self.rotation_angle) - + self.radii[1] * np.sin(t) * np.sin(self.rotation_angle)) + out[:, 1] = (self.center_point[1] + + self.radii[0] * np.cos(t) * np.sin(self.rotation_angle) + + self.radii[1] * np.sin(t) * np.cos(self.rotation_angle)) + return out + + diff --git a/b2ac/geometry/overlap/__init__.py b/b2ac/geometry/overlap/__init__.py new file mode 100644 index 0000000..a5682fb --- /dev/null +++ b/b2ac/geometry/overlap/__init__.py @@ -0,0 +1,2 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- \ No newline at end of file diff --git a/b2ac/geometry/overlap/overlap_functions.py b/b2ac/geometry/overlap/overlap_functions.py new file mode 100644 index 0000000..16b63ba --- /dev/null +++ b/b2ac/geometry/overlap/overlap_functions.py @@ -0,0 +1,595 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +.. module:: overlap_functions + :platform: Unix, Windows + :synopsis: Overlapping functions for the different B2ACGeometricShape objects. + +.. moduleauthor:: hbldh + +Created on 2013-04-15, 11:37 + +""" + +from __future__ import division +from __future__ import print_function +from __future__ import unicode_literals +from __future__ import absolute_import + +import numpy as np + +from b2ac.geometry.point import B2ACPoint +from b2ac.geometry.ellipse import B2ACEllipse +from b2ac.geometry.polygon import B2ACPolygon + +N_POLYPOINTS = 73 + + +def overlap(this, other): + """Returns ratio of overlap between two B2ACGeometricShapes. + + :param this: The first B2ACGeometricShape subclass to check overlap with. + :type this: :py:class:`b2ac.geometry.shape.B2ACGeometricShape` + :param other: The second B2ACGeometricShape subclass to check overlap with. + :type other: :py:class:`b2ac.geometry.shape.B2ACGeometricShape` + :return: Value in [0, 1] describing how much of ``this`` that is enclosed in ``other``. + :rtype: float + + """ + if isinstance(this, B2ACPoint): + if isinstance(other, B2ACPoint): + # Point to point overlap is really non-defined. Using parent class + # __eq__ to test this. + return float(this == other) + elif isinstance(other, B2ACEllipse): + return overlap_point_ellipse(this, other) + elif isinstance(other, B2ACPolygon): + return overlap_point_polygon(this, other) + elif isinstance(this, B2ACEllipse): + if isinstance(other, B2ACPoint): + return overlap_point_ellipse(other, this) + elif isinstance(other, B2ACEllipse): + return overlap_ellipse_ellipse(this, other) + elif isinstance(other, B2ACPolygon): + return overlap_ellipse_polygon(this, other) + elif isinstance(this, B2ACPolygon): + if isinstance(other, B2ACPoint): + return overlap_point_polygon(other, this) + elif isinstance(other, B2ACEllipse): + return overlap_polygon_ellipse(this, other) + elif isinstance(other, B2ACPolygon): + return overlap_polygon_polygon(this, other) + + +def intersection_union_ratio(this, other): + """Returns ratio between the two shapes intersection and their union. + + :param other: The B2ACGeometricShape subclass to check intersection and union ratio with. + :type other: + :param N: The number of points to generate in case of ellipse to polygon conversion is needed. + Default is 73 (one vertex every 5 degrees). + :type N: int + :return: Value in [0, 1]. + :rtype: float + + """ + if isinstance(this, B2ACPoint) or isinstance(other, B2ACPoint): + # Points cannot be used with this function. + return 0. + if isinstance(this, B2ACEllipse): + this_polygon = B2ACPolygon(this.polygonize()) + if isinstance(other, B2ACEllipse): + other_polygon = B2ACPolygon(other.polygonize()) + elif isinstance(other, B2ACPolygon): + other_polygon = other + else: + raise RuntimeError("Invalid comparison object.") + elif isinstance(this, B2ACPolygon): + this_polygon = this + if isinstance(other, B2ACEllipse): + other_polygon = B2ACPolygon(other.polygonize()) + elif isinstance(other, B2ACPolygon): + other_polygon = other + else: + raise RuntimeError("Invalid comparison object.") + else: + # Cannot happen... + raise RuntimeError("Invalid comparison object.") + + intersection_polygon = polygon_intersection(this_polygon, other_polygon) + if intersection_polygon is None: + # No intersection. Return 0. + return 0. + + union_polygon = polygon_union(this_polygon, other_polygon) + return intersection_polygon.get_area() / union_polygon.get_area() + + +def overlap_point_ellipse(point, ellipse): + """Test by definition of ellipse. + + Implementation reference: + `Point in ellipse `_ + + :param point: The point. + :type point: :py:class:`b2ac.geometry.point.B2ACPoint` + :param ellipse: The ellipse. + :type ellipse: :py:class:`b2ac.geometry.ellipse.B2ACEllipse` + :return: 1.0 if point is inside ellipse, 0.0 otherwise. + :rtype: float + + """ + tr_point = point.point - ellipse.center_point + x_coeff = ((np.cos(ellipse.rotation_angle) / ellipse.radii[0]) ** 2 + + (np.sin(ellipse.rotation_angle) / ellipse.radii[1]) ** 2) + xy_coeff = (2 * np.cos(ellipse.rotation_angle) * np.sin(ellipse.rotation_angle) * + ((1 / ellipse.radii[0]) + (1 / ellipse.radii[1]))) + y_coeff = ((np.sin(ellipse.rotation_angle) / ellipse.radii[0]) ** 2 + + (np.cos(ellipse.rotation_angle) / ellipse.radii[1]) ** 2) + return float((x_coeff * (tr_point[0] ** 2) + xy_coeff * (tr_point[0] * tr_point[1]) + + y_coeff * (tr_point[1])) <= 1) + + +def overlap_point_polygon(point, polygon): + """Performs simple ray casting to test if the point is inside the polygon. + + .. note:: + + Can only handle convex polygons! Additionally, if point is on polygon boundary, + this solution might treat it as outside the polygon... + + Method reference: `Ray casting `_ + + Line intersection equation: + + .. math:: + + \\left[ \\begin{array}{c} x_{i} \\\\ y_{i} \\end{array} \\right] + d \\cdot + \\left[ \\begin{array}{c} x_{i+1}-x_{i} \\\\ y_{i+1}-y_{i} \\end{array} \\right] = + \\left[ \\begin{array}{c} \hat{x} \\\\ \hat{y} \\end{array} \\right] + t \\cdot + \\left[ \\begin{array}{c} 1 \\\\ 0 \\end{array} \\right]. + + From this we can solve for :math:`d` and :math:`t` and estimate where + intersection occurs: + + .. math:: + + d = \\frac{\hat{y} - y_{i}}{y_{i+1} - y_{i}} + + .. math:: + + t = x_{i} - \\hat{x} + \\frac{\hat{y} - y_{i}}{y_{i+1} - y_{i}}(x_{i+1} - x_{i}) + + If :math:`d` is smaller than 0, then intersection does not happen at along the polygon + edge, likewise if :math:`d` is greater than the distance between the two vertices. + If it is in between these two, then intersection occurs if :math:`t` is positive, + i.e. it happens in the positive x-axis direction. If edge is parallel, then no crossing + occurs either. + + :param point: The point. + :type point: :py:class:`b2ac.geometry.point.B2ACPoint` + :param polygon: The polygon. + :type polygon: :py:class:`b2ac.geometry.polygon.B2ACPolygon` + :return: 1.0 if point is inside polygon, 0.0 otherwise. + :rtype: float + + """ + # Add first point again to the array of polygon points, to simplify loop. + # Check whether the first and the last point are equal, i.e. the polygon is closed + # properly. Otherwise, add first point to the end of the array to close the polygon. + if np.linalg.norm(polygon.polygon_points[-1, :] - polygon.polygon_points[0, :]) > 1e-10: + pnt_arr = np.concatenate((polygon.polygon_points, [polygon.polygon_points[0, :]])) + else: + pnt_arr = polygon.polygon_points + + # Start ray tracing. + nbr_of_crossings = 0 + for i in xrange(pnt_arr.shape[0] - 1): + # Estimate distance along edge direction that line from point along x-axis + # intersects this polygon edge. + denominator = pnt_arr[i + 1, 1] - pnt_arr[i, 1] + if denominator == 0: + # Parallel lines. + continue + d = (point.point[1] - pnt_arr[i, 1]) / denominator + # The equality means that if intersection occurs at first polygon vertex, it counts + # as a crossing. The omission of it at the "far end" of the edge means that it will + # not count as a crossing at that end. + if (d >= 0.0) and (d < 1.0): + # Have to check if it is in the negative x-direction the crossing occurs; + # if so we can neglect this. + t = pnt_arr[i, 0] - point.point[0] + d * (pnt_arr[i + 1, 0] - pnt_arr[i, 0]) + # Greater than 0 means that this point counts as "being on the right hand + # side" of the edge. + if t > 0.0: + nbr_of_crossings += 1 + + # If crossings of edges only occurs once, then the point must lie inside the + # convex polygon. If 0 or 2 times, it is outside. + if nbr_of_crossings == 1: + return 1.0 + else: + return 0.0 + + +def overlap_ellipse_ellipse(e1, e2): + """Calculates the intersection of two ellipses, returning the ratio of how + much of ``e1`` that is contained in ``e2``. + + Current implementation: + Converts both ellipses to polygons and uses :py:meth:`overlap_polygon_polygon`. + + .. note:: + + Overlap with itthis should return 1.0, but is currently suffering from some + numerical instabilities that returns unstable results! + + Implementation proposals: + + 1. http://www.geometrictools.com/Documentation/IntersectionOfEllipses.pdf + 2. http://www.geometrictools.com/Documentation/AreaIntersectingEllipses.pdf + + :param e1: The first ellipse. + :type e1: :py:class:`b2ac.geometry.ellipse.B2ACEllipse` + :param e1: The second ellipse. + :type e2: :py:class:`b2ac.geometry.ellipse.B2ACEllipse` + :return: Value in [0, 1] describing how much of ``e1`` + that is contained in ``e2``. + :rtype: float + + """ + return overlap_polygon_polygon(B2ACPolygon(e1.polygonize(N_POLYPOINTS)), + B2ACPolygon(e2.polygonize(N_POLYPOINTS))) + + +def overlap_ellipse_polygon(ellipse, polygon): + """Get ratio of how much of ``ellipse`` that is contained in ``polygon``. + + Current implementation: + Convert ellipse to polygon and use :py:meth:`overlap_polygon_polygon` + + :param ellipse: The ellipse. + :type ellipse: :py:class:`b2ac.geometry.ellipse.B2ACEllipse` + :param polygon: The polygon. + :type polygon: :py:class:`b2ac.geometry.polygon.B2ACPolygon` + :return: Value in [0, 1] describing how much of ``ellipse`` + that is contained in ``polygon``. + :rtype: float + + """ + + return overlap_polygon_polygon(B2ACPolygon(ellipse.polygonize(N_POLYPOINTS)), polygon) + + +def overlap_polygon_ellipse(polygon, ellipse): + """Get ratio of how much of ``polygon`` that is contained in ``ellipse``. + + Current implementation: + Convert ellipse to polygon and use :py:meth:`overlap_polygon_polygon` + + :param polygon: The polygon. + :type polygon: :py:class:`b2ac.geometry.polygon.B2ACPolygon` + :param ellipse: The ellipse. + :type ellipse: :py:class:`b2ac.geometry.ellipse.B2ACEllipse` + :return: Value in [0, 1] describing how much of ``polygon`` + that is contained in ``ellipse``. + :rtype: float + + """ + return overlap_polygon_polygon(polygon, B2ACPolygon(ellipse.polygonize(N_POLYPOINTS))) + + +def overlap_polygon_polygon(p1, p2): + """Get ratio of how much of ``p1`` that is contained in ``p2``. + + .. note:: + + Can only handle convex polygons since it uses Sutherland-Hodgman algorithm! + + Method reference: + `Sutherland-Hodgman `_ + + :param p1: The first polygon. + :type p1: :py:class:`b2ac.geometry.polygon.B2ACPolygon` + :param p2: The second polygon + :type p2: :py:class:`b2ac.geometry.polygon.B2ACPolygon` + :return: Value in [0, 1] describing how much of ``p1`` + that is contained in ``p2``. + :rtype: float + + """ + # First, check if the polygons are equal. If they are, the intersection calculation + # might encounter divide-by-zero errors and is furthermore unnecessary to run... + if np.allclose(p1.polygon_points.shape, p2.polygon_points.shape) and \ + np.allclose(p1.polygon_points, p2.polygon_points): + return 1.0 + + # Return the ratio of size of the intersection and size of the subject polygon. + intersection_polygon = polygon_intersection(p1, p2) + if intersection_polygon is None: + return 0. + else: + return intersection_polygon.get_area() / p1.get_area() + + +def polygon_intersection(p1, p2): + """Returns the polygon representing the intersection of two other. + + .. note:: + + Can only handle convex polygons since it uses Sutherland-Hodgman algorithm! + + Method reference: + `Sutherland-Hodgman `_ + + :param p1: The first polygon. + :type p1: :py:class:`b2ac.geometry.polygon.B2ACPolygon` + :param p2: The second polygon + :type p2: :py:class:`b2ac.geometry.polygon.B2ACPolygon` + :return: The intersection polygon or None if no intersection exists. + :rtype: :py:class:`b2ac.geometry.polygon.B2ACPolygon` or :py:class:`None`. + + """ + # First remove the "closing points" of the polygon if such a vertex is present. + p1_pnts = p1.get_open_polygon() + p2_pnts = p2.get_open_polygon() + + # Perform clipping of polygons and create a new polygon instance from that. + # TODO: Write function for ordering points clockwise in standard Cartesian plane. + clipped_polygon_points = sutherland_hodgman_polygon_clipping(p1_pnts, p2_pnts) + if clipped_polygon_points is not None and len(clipped_polygon_points): + return p1.__class__(clipped_polygon_points) + else: + return None + + +def sutherland_hodgman_polygon_clipping(subject_polygon, clip_polygon): + """Sutherland-Hodgman polygon clipping. + + .. note + + This algorithm works in regular Cartesian plane, not in inverted y-axis image plane, + so make sure that polygons sent in are ordered clockwise in regular Cartesian sense! + + Method reference: + `Sutherland-Hodgman `_ + + Reference code found at `Rosettacode + `_ + + :param subject_polygon: A [n x 2] array of points representing the non-closed polygon to reduce. + :type subject_polygon: :py:class:`numpy.ndarray` + :param clip_polygon: A [m x 2] array of points representing the non-closed polygon to clip with. + :type clip_polygon: :py:class:`numpy.ndarray` + :return: A [r x 2] array of points representing the intersection polygon or :py:class:`None` + if no intersection is present. + :rtype: :py:class:`numpy.ndarray` or :py:class:`None` + + """ + TOLERANCE = 1e-14 + + def inside(p): + # This ``inside`` function assumes y-axis pointing upwards. If one would + # like to rewrite this function to work with clockwise ordered coordinates + # in the image style, then reverse the comparison from ``>`` to ``<``. + return (cp2[0]-cp1[0])*(p[1]-cp1[1]) > (cp2[1]-cp1[1])*(p[0]-cp1[0]) + + def compute_intersection(): + dc = [cp1[0] - cp2[0], cp1[1] - cp2[1]] + dp = [s[0] - e[0], s[1] - e[1]] + n1 = cp1[0] * cp2[1] - cp1[1] * cp2[0] + n2 = s[0] * e[1] - s[1] * e[0] + denominator = (dc[0] * dp[1] - dc[1] * dp[0]) + if np.abs(denominator) < TOLERANCE: + # Lines were parallel. + return None + n3 = 1.0 / denominator + return [(n1*dp[0] - n2*dc[0]) * n3, (n1*dp[1] - n2*dc[1]) * n3] + + output_list = list(subject_polygon) + cp1 = clip_polygon[-1] + + for clip_vertex in clip_polygon: + cp2 = clip_vertex + input_list = output_list + if not input_list: + return None + output_list = [] + s = input_list[-1] + + for subject_vertex in input_list: + e = subject_vertex + if inside(e): + if not inside(s): + intersection = compute_intersection() + if intersection is not None: + output_list.append(intersection) + output_list.append(e) + elif inside(s): + intersection = compute_intersection() + if intersection is not None: + output_list.append(intersection) + s = e + cp1 = cp2 + + # TODO: Verify that points are clockwise sorted here. + pnts_out = [] + while len(output_list): + pnt = output_list.pop(0) + if not any([np.all(np.equal(pnt, unique_pnt)) for unique_pnt in pnts_out]): + pnts_out.append(pnt) + + return np.array(pnts_out) + + +def polygon_union(p1, p2, use_graham_scan=False): + """Calculates the union of two polygons and returns this union polygon. + + Implementation proposals: + + 1. `Polygon union `_ + 2. Find all intersection points (Sutherland-Hodgman) to get a point cloud and + then run the `Graham scan `_ algorithm + on that set of points. + + :param p1: The first polygon. + :type p1: :py:class:`b2ac.geometry.polygon.B2ACPolygon` + :param p2: The second polygon + :type p2: :py:class:`b2ac.geometry.polygon.B2ACPolygon` + :param use_graham_scan: Boolean that can be used for selecting the slower Graham Scan convex hull algorithm + instead of Quickhull. + :type use_graham_scan: bool + :return: The union polygon or :py:class:`None` if the polygons are not connected. + :rtype: :py:class:`b2ac.geometry.polygon.B2ACPolygon` or :py:class:`None` + + """ + p_intersection = polygon_intersection(p1, p2) + if p_intersection is None: + return None + points = np.concatenate((p1.polygon_points, + p2.polygon_points, + p_intersection.polygon_points)) + if use_graham_scan: + return p1.__class__(graham_scan(points)) + else: + return p1.__class__(quickhull(points)) + + +def graham_scan(points): + """Calculates the convex hull of an arbitrary 2D point cloud. + + Method reference: + `Graham scan `_ + + Code adapted from: + `Google Code `_ + + :param points: A [n x 2] array of points from which to estimate the convex hull. + :type points: :py:class:`numpy.ndarray` + :return: A [m x 2] array defining the convex hull polygon of the points sent in. + :rtype: :py:class:`numpy.ndarray` + + """ + def angle_cmp(pivot): + """Receive a coordinate as the pivot and return a + function for comparing angles formed by another + two coordinates around the pivot. + """ + def _angle_cmp(c1, c2): + v1 = c1[0] - pivot[0], c1[1] - pivot[1] + v2 = c2[0] - pivot[0], c2[1] - pivot[1] + cp = np.cross(v1, v2) + if cp < 0: + return 1 + elif cp == 0: + return 0 + else: + return -1 + return _angle_cmp + + def turning(c1, c2, c3): + """Determine which way does c1 -> c2 -> c3 turns.""" + v1 = c2[0] - c1[0], c2[1] - c1[1] + v2 = c3[0] - c2[0], c3[1] - c2[1] + cp = np.cross(v1, v2) + if cp < 0: + return 'RIGHT' + elif cp == 0: + return 'STRAIGHT' + else: + return 'LEFT' + + def point_cmp(p1, p2): + """Compares 2D points with regard to y coordinate value first, then x.""" + cmp_val = cmp(p1[1], p2[1]) + if cmp_val == 0: + return cmp(p1[0], p2[0]) + else: + return cmp_val + + num = len(points) + if num < 3: + raise Exception('Too few coordinates sent in.') + + # sort the coords according to y + points = sorted(points.tolist(), cmp=point_cmp) + + # select the leftmost coord as the pivot + pivot = points[0] + coords = points[1:] + + # for remaining coords, sort them by polar angle + # in counterclockwise order around pivot + coords.sort(angle_cmp(pivot)) + + # push the first three coords in a stack + stack = [pivot, coords[0], coords[1]] + + # for the rest of the coords, while the angle formed by + # the coord of the next-to-top of the stack, coord of + # top of stack and the next coord makes a nonleft turn, + # pop the stack + # also, push the next coord into the stack at each loop + for i in range(2, num - 1): + while len(stack) >= 2 and \ + turning(stack[-2], stack[-1], coords[i]) != 'LEFT': + stack = stack[:-1] + stack.append(coords[i]) + + return np.array(stack) + + +def quickhull(sample): + """Calculates the convex hull of an arbitrary 2D point cloud. + + This is a pure Python version of the Quick Hull algorithm. + It's based on the version of ``literateprograms``, but fixes some + old-style Numeric function calls. + + This version works with numpy version > 1.2.1 + + References: + + * `Literateprograms `_ + * `Wikipedia `_ + + Code adapted from: + + ``_ + + :param sample: Points to which the convex hull is desired to be found. + :type sample: :py:class:`numpy.ndarray` + :return: The convex hull of the points. + :rtype: :py:class:`numpy.ndarray` + + """ + + def calculate_convex_hull(sample): + link = lambda a, b: np.concatenate((a, b[1:])) + edge = lambda a, b: np.concatenate(([a], [b])) + + def dome(sample, base): + h, t = base + dists = np.dot(sample-h, np.dot(((0, -1), (1, 0)), (t - h))) + outer = np.repeat(sample, dists > 0, axis=0) + + if len(outer): + pivot = sample[np.argmax(dists)] + return link(dome(outer, edge(h, pivot)), + dome(outer, edge(pivot, t))) + else: + return base + + if len(sample) > 2: + axis = sample[:, 0] + base = np.take(sample, [np.argmin(axis), np.argmax(axis)], axis=0) + return link(dome(sample, base), + dome(sample, base[::-1])) + else: + return sample + + # Perform a reversal of points here to get points ordered clockwise instead of + # counter clockwise that the QuickHull above returns. + return calculate_convex_hull(sample)[::-1, :] + + diff --git a/b2ac/geometry/point.py b/b2ac/geometry/point.py new file mode 100644 index 0000000..657660e --- /dev/null +++ b/b2ac/geometry/point.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +:mod:`point` +================== + +.. module:: point + :platform: Unix, Windows + :synopsis: + +.. moduleauthor:: hbldh + +Created on 2015-09-24, 23:51 + +""" + +from __future__ import division +from __future__ import print_function +from __future__ import unicode_literals +from __future__ import absolute_import + +import numpy as np + +from b2ac.geometry.shape import B2ACGeometricShape + + +class B2ACPoint(B2ACGeometricShape): + """A class for representing points. + + :param point: An array of two values: x and y cooedinate for point. + :type point: list, tuple or :py:class:`numpy.ndarray` + + Example Input: + + .. code-block:: python + + {u'algorithm': u'Is1FpgaGlintPupilDetector', + u'name': u'glint', + u'points': [{u'x': 453, u'y': 215}], + u'type': u'point'} + + """ + + def __init__(self, point): + """Constructor for B2ACEllipse""" + super(B2ACPoint, self).__init__() + if len(point) != 2: + raise ValueError("Only 2D-points supported.") + self.point = np.array(point, 'float') + + def __str__(self): + return "Point: ({0:.2f}, {1:.2f})".format(*self.point) + + def __repr__(self): + return str(self) + + def __eq__(self, other): + if isinstance(self, other): + return np.testing.assert_allclose(self.point, other.point) + return False + + @property + def mpl_patch_arguments(self): + """Returns Matplotlib Patch arguments. Can then be plotted by with following + snippet: + + .. code-block:: python + + import matplotlib.pyplot as plt + from matplotlib.patches import Point + from b2ac.geometry.point import B2ACPoint + + p = B2ACPoint((3.0, 5.0)) + plt.gca().add_patch(Point(p.mpl_patch_arguments, color='g')) + plt.show() + + """ + return self.point, + + def get_center_point(self): + """Returns the B2ACPoints value.""" + return self.point + + def get_area(self): + """Returns the area covered of the B2ACPoint.""" + return 0. diff --git a/b2ac/geometry/polygon.py b/b2ac/geometry/polygon.py new file mode 100644 index 0000000..bb089cf --- /dev/null +++ b/b2ac/geometry/polygon.py @@ -0,0 +1,171 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +:mod:`polygon` +================== + +.. module:: polygon + :platform: Unix, Windows + :synopsis: + +.. moduleauthor:: hbldh + +Created on 2015-09-24, 23:50 + +""" + +from __future__ import division +from __future__ import print_function +from __future__ import unicode_literals +from __future__ import absolute_import + +import warnings +import numpy as np + +from b2ac.geometry.shape import B2ACGeometricShape + + +class B2ACPolygon(B2ACGeometricShape): + """A class for representing polygons.""" + + def __init__(self, points): + """Constructor for B2ACPolygon""" + super(B2ACPolygon, self).__init__() + + self.polygon_points = np.array(points, 'float') + if self.polygon_points.shape[1] != 2: + raise ValueError("Polygon must be entered as a [n x 2] array, i.e. a 2D polygon.") + + def __str__(self): + return "Polygon with {0} vertices:\n{1}".format( + self.polygon_points.shape[0], self.polygon_points) + + def __repr__(self): + return str(self) + + @property + def mpl_patch_arguments(self): + """Returns Matplotlib patch arguments. Can then be plotted by with following + snippet: + + .. code-block:: python + + import matplotlib.pyplot as plt + from matplotlib.patches import Polygon + from b2ac.geometry.polygon import B2ACPolygon + + p = B2ACPolygon([(2.1, 1.2), (5.2, 2.4), (1.0, 0.2)]) + plt.gca().add_patch(Polygon( + p.mpl_patch_arguments, closed=True, fill=False, edgecolor='r') + plt.show() + + """ + return self.polygon_points, + + def get_center_point(self, use_centroid=True): + """Returns a center of weight for the object. + + :param use_centroid: Uses a centroid finding method instead of pure mean of vertices. + :type use_centroid: bool + + """ + if use_centroid: + with warnings.catch_warnings(record=False) as w: + # Cause all warnings to never be triggered. + warnings.simplefilter("ignore") + + pnt_array = self.get_closed_polygon() + + A = self._area_help_function() + D = (pnt_array[:-1, 0] * pnt_array[1:, 1] - + pnt_array[1:, 0] * pnt_array[:-1, 1]) + + c_x = ((pnt_array[:-1, 0] + pnt_array[1:, 0]) * D).sum() / (6 * A) + c_y = ((pnt_array[:-1, 1] + pnt_array[1:, 1]) * D).sum() / (6 * A) + + if np.isnan(c_x) or np.isinf(c_x) or np.isnan(c_y) or np.isinf(c_y): + # If centroid calculations fails (e.g. due to zero-valued area) then use the + # mean of the vertices as center point instead. + return np.mean(self.get_open_polygon(), 0) + else: + return np.array([c_x, c_y]) + else: + return np.mean(self.get_open_polygon(), 0) + + def get_area(self): + """Returns the area covered by the B2ACPolygon. + + :return: The area of the polygon. + :rtype: float + + """ + # Abs to handle counter-clockwise ordering. + return np.abs(self._area_help_function()) + + @property + def is_clockwise_ordered(self): + """Property for checking if the polygon points are ordered clockwise. + + :return: Clockwise ordering status. + :rtype: bool + + """ + return self._area_help_function() > 0 + + @property + def is_counter_clockwise_ordered(self): + """Property for checking if the polygon points are ordered counter-clockwise. + + :return: Counter-clockwise ordering status. + :rtype: bool + + """ + return self._area_help_function() < 0 + + def get_bounding_box(self): + """Get a four point Polygon describing the bounding box of the current Polygon. + + :return: A new polygon, describing this one's bounding box. + :rtype: :py:class:`b2ac.geometry.polygon.B2ACPolygon` + + """ + mins = self.polygon_points.min(axis=0) + maxs = self.polygon_points.max(axis=0) + bb_pnts = np.zeros((4, 2), dtype=self.polygon_points.dtype) + bb_pnts[0, :] = mins + bb_pnts[1, :] = [mins[0], maxs[1]] + bb_pnts[2, :] = maxs + bb_pnts[3, :] = [maxs[0], mins[1]] + out = B2ACPolygon(bb_pnts) + return out + + def _area_help_function(self): + """Performing the actual area calculation. + + :return: The area of the polygon, negative if counter-clockwise orientation of polygon points. + :rtype: float + + """ + # If the polygon is not closed, append first point to the end of polygon to ensure that. + pnt_array = self.get_closed_polygon() + + return (pnt_array[:-1, 0] * pnt_array[1:, 1] - + pnt_array[1:, 0] * pnt_array[:-1, 1]).sum() / 2 + + def get_closed_polygon(self): + """Appends the first point to the end of point array, in order to "close" the polygon.""" + if not self.is_closed: + return np.concatenate([self.polygon_points, [self.polygon_points[0, :]]]) + else: + return self.polygon_points + + def get_open_polygon(self): + """Removes the last point if it is close enough to the first, in order to "open" the polygon.""" + if self.is_closed: + return self.polygon_points[:-1, :] + else: + return self.polygon_points + + @property + def is_closed(self): + return np.linalg.norm(self.polygon_points[0, :] - self.polygon_points[-1, :]) < 1e-10 diff --git a/b2ac/geometry/shape.py b/b2ac/geometry/shape.py new file mode 100644 index 0000000..f4b1bc0 --- /dev/null +++ b/b2ac/geometry/shape.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +:mod:`shape` +================== + +.. module:: shape + :platform: Unix, Windows + :synopsis: + +.. moduleauthor:: hbldh + +Created on 2015-09-24, 23:51 + +""" + +from __future__ import division +from __future__ import print_function +from __future__ import unicode_literals +from __future__ import absolute_import + +#from b2ac.geometry.overlap.overlap_functions import overlap +#from b2ac.geometry.distance.distance_functions import distance + + +class B2ACGeometricShape(object): + """Parent class for geometric shapes.""" + + def __init__(self): + """Constructor for parent class.""" + pass + + def __eq__(self, other): + """Equality operator.""" + # TODO: Evaluate better equality measure... + self.distance(other) < 0.01 + + def __lt__(self, other): + # TODO: Do these have any reasonable meaning? + raise NotImplementedError() + + def __le__(self, other): + # TODO: Do these have any reasonable meaning? + raise NotImplementedError() + + def __gt__(self, other): + # TODO: Do these have any reasonable meaning? + raise NotImplementedError() + + def __ge__(self, other): + # TODO: Do these have any reasonable meaning? + raise NotImplementedError() + + @property + def mpl_patch_arguments(self): + """Returns Matplotlib Patch arguments.""" + raise NotImplementedError() + + def get_center_point(self): + """Returns a center of weight for the object.""" + raise NotImplementedError() + + def get_area(self): + """Returns the area covered of the B2ACGeometricShape object.""" + raise NotImplementedError() diff --git a/b2ac/matrix/__init__.py b/b2ac/matrix/__init__.py index cb66d2d..a5682fb 100644 --- a/b2ac/matrix/__init__.py +++ b/b2ac/matrix/__init__.py @@ -1 +1,2 @@ -__author__ = 'Henrik Blidh' +#!/usr/bin/env python +# -*- coding: utf-8 -*- \ No newline at end of file diff --git a/b2ac/matrix/fixed_point.py b/b2ac/matrix/fixed_point.py index 0593279..17f5acc 100644 --- a/b2ac/matrix/fixed_point.py +++ b/b2ac/matrix/fixed_point.py @@ -1,7 +1,7 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- """ -:mod:`fixed_point` -- +:mod:`fixed_point` -- ====================== .. module:: fixed_point @@ -52,4 +52,23 @@ def scale_64bit_vector(v): value >>= 1 scale += 1 - return v >> scale, scale \ No newline at end of file + return v >> scale, scale + + +def scale_T_matrix(T_no_det, det_S3): + m, M = np.abs(T_no_det).min(), np.abs(T_no_det).max() + if np.log2(M) <= 32: + scale = 0 + elif np.log2(M) <= 40: + scale = 8 + elif np.log2(M) <= 48: + scale = 16 + elif np.log2(M) <= 56: + scale = 24 + else: + scale = 32 + + det_S3 >>= scale + T_no_det >>= scale + + return T_no_det, det_S3 diff --git a/b2ac/matrix/matrix_ref.py b/b2ac/matrix/matrix_ref.py new file mode 100644 index 0000000..fdd3ef9 --- /dev/null +++ b/b2ac/matrix/matrix_ref.py @@ -0,0 +1,230 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +.. module:: matrix + :platform: Unix, Windows + :synopsis: Operations on matrices. + +.. moduleauthor:: hbldh + +Created on 2013-05-15, 10:45 + +""" + +from __future__ import division +from __future__ import print_function +from __future__ import unicode_literals +from __future__ import absolute_import + +import numpy as np + + +def inverse_symmetric_3by3_double(M): + """C style inverse of a symmetric, flattened 3 by 3 matrix. + + Returns the adjoint matrix and the determinant, s.t. + + .. math:: + + M^{-1} = \\frac{1}{\\det(M)} \\cdot \\text{adj}(M). + + + For integer matrices, this then returns exact results by avoiding + to apply the division. + + :param M: The matrix to find inverse to. Assumes array with shape (6,). + :type M: :py:class:`numpy.ndarray` + :return: The inverse matrix, flattened. + :rtype: :py:class:`numpy.ndarray` + + """ + + determinant = 0 + inverse = np.zeros((9,), dtype='float') + + # First row of adjunct matrix + inverse[0] = (M[3] * M[5] - (M[4] ** 2)) # Det #0 + inverse[1] = -(M[1] * M[5] - M[4] * M[2]) # Det #1 + inverse[2] = (M[1] * M[4] - M[3] * M[2]) # Det #2 + + # Second row of adjunct matrix + inverse[3] = inverse[1] + inverse[4] = (M[0] * M[5] - (M[2] ** 2)) + inverse[5] = -(M[0] * M[4] - M[1] * M[2]) + + # Third row of adjunct matrix + inverse[6] = inverse[2] + inverse[7] = inverse[5] + inverse[8] = (M[0] * M[3] - (M[1] ** 2)) + + determinant += M[0] * inverse[0] + determinant += M[1] * inverse[1] # Using addition since minus is integrated in adjunct matrix. + determinant += M[2] * inverse[2] + + for i in xrange(len(inverse)): + inverse[i] /= determinant + + return inverse + + +def inverse_symmetric_3by3_int(M): + """C style inverse of a symmetric, flattened 3 by 3 matrix. + + Returns the adjoint matrix and the determinant, s.t. + + .. math:: + + M^{-1} = \\frac{1}{\\det(M)} \\cdot \\text{adj}(M). + + + For integer matrices, this then returns exact results by avoiding + to apply the division. + + :param M: The matrix to find inverse to. Assumes array with shape (6,). + :type M: :py:class:`numpy.ndarray` + :return: The adjoint flattened matrix and the determinant. + :rtype: tuple + + """ + + determinant = 0 + adj_M = np.zeros((9,), dtype='int32') + + # First row of adjunct matrix + adj_M[0] = (M[3] * M[5] - (M[4] ** 2)) # Det #0 + adj_M[1] = -(M[1] * M[5] - M[4] * M[2]) # Det #1 + adj_M[2] = (M[1] * M[4] - M[3] * M[2]) # Det #2 + + # Second row of adjunct matrix + adj_M[3] = adj_M[1] + adj_M[4] = (M[0] * M[5] - (M[2] ** 2)) + adj_M[5] = -(M[0] * M[4] - M[1] * M[2]) + + # Third row of adjunct matrix + adj_M[6] = adj_M[2] + adj_M[7] = adj_M[5] + adj_M[8] = (M[0] * M[3] - (M[1] ** 2)) + + determinant += np.int64(M[0]) * np.int64(adj_M[0]) + determinant += np.int64(M[1]) * np.int64(adj_M[1]) # Using addition since minus is integrated in adjunct matrix. + determinant += np.int64(M[2]) * np.int64(adj_M[2]) + + return adj_M, determinant + + +def inverse_3by3_int(M): + """C style inverse of a flattened 3 by 3 matrix. + + Returns the adjoint matrix and the determinant, s.t. + + .. math:: + + M^{-1} = \\frac{1}{\\det(M)} \\cdot \\text{adj}(M). + + + For integer matrices, this then returns exact results by avoiding + to apply the division. + + :param M: The matrix to find inverse to. + :type M: :py:class:`numpy.ndarray` + :return: The adjoint flattened matrix and the determinant. + :rtype: tuple + + """ + if len(M.shape) > 1: + M = M.flatten() + + determinant = 0 + adj_M = np.zeros((9,), 'int') + + # First row of adjunct matrix + adj_M[0] = (M[4] * M[8] - M[7] * M[5]) # Det #0 + adj_M[1] = -(M[1] * M[8] - M[7] * M[2]) + adj_M[2] = (M[1] * M[5] - M[4] * M[2]) + + # Second row of adjunct matrix + adj_M[3] = -(M[3] * M[8] - M[6] * M[5]) # Det #1 + adj_M[4] = (M[0] * M[8] - M[6] * M[2]) + adj_M[5] = -(M[0] * M[5] - M[3] * M[2]) + + # Third row of adjunct matrix + adj_M[6] = (M[3] * M[7] - M[6] * M[4]) # Det #2 + adj_M[7] = -(M[0] * M[7] - M[6] * M[1]) + adj_M[8] = (M[0] * M[4] - M[3] * M[1]) + + determinant += np.int64(M[0]) * np.int64(adj_M[0]) + determinant += np.int64(M[1]) * np.int64(adj_M[3]) # Using addition since minus is integrated in adjunct matrix. + determinant += np.int64(M[2]) * np.int64(adj_M[6]) + + return adj_M, determinant + + +def inverse_3by3_double(M): + """C style inverse of a flattened 3 by 3 matrix. + + .. math:: + + M^{-1} = \\frac{1}{\\det(M)} \\cdot \\text{adj}(M). + + + For integer matrices, this then returns exact results by avoiding + to apply the division. + + :param M: The matrix to find inverse to. + :type M: :py:class:`numpy.ndarray` + :return: The inverse matrix. + :rtype: :py:class:`numpy.ndarray` + + """ + if len(M.shape) > 1: + M = M.flatten() + + determinant = 0 + adj_M = np.zeros((9,), 'int') + + # First row of adjunct matrix + adj_M[0] = (M[4] * M[8] - M[7] * M[5]) # Det #0 + adj_M[1] = -(M[1] * M[8] - M[7] * M[2]) + adj_M[2] = (M[1] * M[5] - M[4] * M[2]) + + # Second row of adjunct matrix + adj_M[3] = -(M[3] * M[8] - M[6] * M[5]) # Det #1 + adj_M[4] = (M[0] * M[8] - M[6] * M[2]) + adj_M[5] = -(M[0] * M[5] - M[3] * M[2]) + + # Third row of adjunct matrix + adj_M[6] = (M[3] * M[7] - M[6] * M[4]) # Det #2 + adj_M[7] = -(M[0] * M[7] - M[6] * M[1]) + adj_M[8] = (M[0] * M[4] - M[3] * M[1]) + + determinant += M[0] * adj_M[0] + determinant += M[1] * adj_M[3] # Using addition since minus is integrated in adjunct matrix. + determinant += M[2] * adj_M[6] + + return adj_M / determinant + + +def add_symmetric_matrix(M, M_sym): + """Add a regular matrix and a symmetric one. + + :param M: A [3x3] matrix to add with symmetric matrix. + :type M: :py:class:`numpy.ndarray` + :param M_sym: A [6x1] array to add with M. + :type M_sym: :py:class:`numpy.ndarray` + :return: The sum of the two matrices. + :rtype: :py:class:`numpy.ndarray` + + """ + M[0, 0] += M_sym[0] + M[0, 1] += M_sym[1] + M[1, 0] += M_sym[1] + M[0, 2] += M_sym[2] + M[2, 0] += M_sym[2] + + M[1, 1] += M_sym[3] + M[1, 2] += M_sym[4] + M[2, 1] += M_sym[4] + + M[2, 2] += M_sym[5] + + return M diff --git a/b2ac/preprocess.py b/b2ac/preprocess.py new file mode 100644 index 0000000..5e1a9b2 --- /dev/null +++ b/b2ac/preprocess.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +:mod:`preprocess` +================== + +.. module:: preprocess + :platform: Unix, Windows + :synopsis: + +.. moduleauthor:: hbldh + +Created on 2015-10-13 + +""" + +from __future__ import division +from __future__ import print_function +from __future__ import unicode_literals +from __future__ import absolute_import + + +def remove_mean_values(points): + """Calculate integer mean values of the 2D array and removing it from the data. + + :param points: The points defining the ellipse. + :type points: + :return: Points, mean X integer coordinate, mean Y integer coordinate + :rtype: tuple + + """ + x_mean = int(points[:, 0].mean()) + y_mean = int(points[:, 1].mean()) + return points - (x_mean, y_mean), x_mean, y_mean \ No newline at end of file diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..50daa83 --- /dev/null +++ b/requirements.txt @@ -0,0 +1 @@ +numpy>=1.6 diff --git a/setup.py b/setup.py index 5bb6489..f416970 100644 --- a/setup.py +++ b/setup.py @@ -17,46 +17,39 @@ from __future__ import print_function from __future__ import absolute_import -from setuptools import setup, Extension +from setuptools import setup, Extension, find_packages import numpy -ellipse_fitting_module = Extension('ellipse_fitter', - sources=['b2ac/ext/src/ellipse_fitter.c', - 'b2ac/ext/src/Eigenmethods_double.c', - 'b2ac/ext/src/Eigenmethods_float.c', - 'b2ac/ext/src/Eigenmethods_int.c', - 'b2ac/ext/src/EllipseFit.c', - 'b2ac/ext/src/EllipseFit_double.c', - 'b2ac/ext/src/EllipseFit_float.c', - 'b2ac/ext/src/EllipseFit_int.c', - ], - include_dirs=[numpy.get_include(), - 'b2ac/ext/src/']) setup( name='b2ac', - version='0.1.0', - description='Python and C implementations of an ellipse fitting algorithm in double and fixed point precision.', + version='0.2.0', author='Henrik Blidh', author_email='henrik.blidh@nedomkull.com', + description='Python and C implementations of an ellipse fitting algorithm in double and fixed point precision.', + long_description="TBD", license='MIT', - url='https://github.com/hbldh/ellipse-fitting', - packages=[ - 'b2ac', - 'b2ac.fit', - 'b2ac.fit.eigenmethods', - 'b2ac.fit.matrix', - 'b2ac.ext', - ], + url='https://github.com/hbldh/b2ac', + packages=find_packages(), + install_requires=[line.strip() for line in open("requirements.txt")], package_data={'b2ac.ext': [ 'src/*', '*.cfg', '*.py']}, - install_requires=[ - 'numpy>=1.6', - ], dependency_links=[], + ext_package='b2ac.ext', ext_modules=[ - ellipse_fitting_module, + Extension('ellipse_fitter', + sources=['b2ac/ext/src/ellipse_fitter.c', + 'b2ac/ext/src/Eigenmethods_double.c', + 'b2ac/ext/src/Eigenmethods_float.c', + 'b2ac/ext/src/Eigenmethods_int.c', + 'b2ac/ext/src/EllipseFit.c', + 'b2ac/ext/src/EllipseFit_double.c', + 'b2ac/ext/src/EllipseFit_float.c', + 'b2ac/ext/src/EllipseFit_int.c', + ], + include_dirs=[numpy.get_include(), + 'b2ac/ext/src/']), ], ) diff --git a/tests/__init__.py b/tests/__init__.py new file mode 100644 index 0000000..66339bd --- /dev/null +++ b/tests/__init__.py @@ -0,0 +1 @@ +__author__ = 'henri' diff --git a/tests/test_double.py b/tests/test_double.py new file mode 100644 index 0000000..e52ecce --- /dev/null +++ b/tests/test_double.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +:mod:`test_ellipse_fit_reference` +================== + +.. module:: test_ellipse_fit_reference + :platform: Unix, Windows + :synopsis: + +.. moduleauthor:: hbldh + +Created on 2015-10-02 + +""" + +from __future__ import division +from __future__ import print_function +from __future__ import unicode_literals +from __future__ import absolute_import + +import numpy as np +import b2ac.ext.ellipse_fitter as fitext + +import b2ac.fit.reference as efr +from b2ac.fit import fit_improved_B2AC_double +import b2ac.conversion as c2gconv +from b2ac.geometry.ellipse import B2ACEllipse +from b2ac.geometry.overlap.overlap_functions import overlap + + +# Bengt Sändh FInn Zetterholm + + +class TestReferenceImplementation(object): + + def setup(self): + self.e = B2ACEllipse(center=(50.0, 75.0), radii=(50.0, 20.0), rotation_angle=0.707) + self.points = np.array(self.e.polygonize(), 'int32') + +# def test_fit_numpy_version(self): +# # Fails +# output = fit.fit_improved_B2AC_numpy(self.points.copy()) +# e_fitted = B2ACEllipse(*output) +# assert np.linalg.norm(self.e.center_point - e_fitted.center_point) < 1 +# assert np.linalg.norm(max(self.e.radii) - max(e_fitted.radii)) < 0.25 +# assert np.linalg.norm(min(self.e.radii) - min(e_fitted.radii)) < 0.25 +# assert overlap(self.e, e_fitted) > 0.98 +# assert overlap(e_fitted, self.e) > 0.98 + + def test_fit_double_version(self): + output = fit_improved_B2AC_double(self.points.copy()) + output = c2gconv.conic_to_general_1(output) + e_fitted = B2ACEllipse(*output) + assert np.linalg.norm(self.e.center_point - e_fitted.center_point) < 1 + assert np.linalg.norm(max(self.e.radii) - max(e_fitted.radii)) < 0.25 + assert np.linalg.norm(min(self.e.radii) - min(e_fitted.radii)) < 0.25 + assert overlap(self.e, e_fitted) > 0.98 + assert overlap(e_fitted, self.e) > 0.98 + + def test_fit_ext_double_version(self): + output = fitext.fit_ellipse_double(self.points.copy()) + e_fitted = B2ACEllipse(*output) + assert np.linalg.norm(self.e.center_point - e_fitted.center_point) < 1 + assert np.linalg.norm(max(self.e.radii) - max(e_fitted.radii)) < 0.25 + assert np.linalg.norm(min(self.e.radii) - min(e_fitted.radii)) < 0.25 + assert overlap(self.e, e_fitted) > 0.98 + assert overlap(e_fitted, self.e) > 0.98 + + def test_fit_ext_float_version(self): + output = fitext.fit_ellipse_float(self.points.copy()) + e_fitted = B2ACEllipse(*output) + assert np.linalg.norm(self.e.center_point - e_fitted.center_point) < 1 + assert np.linalg.norm(max(self.e.radii) - max(e_fitted.radii)) < 0.25 + assert np.linalg.norm(min(self.e.radii) - min(e_fitted.radii)) < 0.25 + assert overlap(self.e, e_fitted) > 0.98 + assert overlap(e_fitted, self.e) > 0.98 + + def test_fit_ref(self): + points = self.points.copy() + output = efr.fit_improved_B2AC(points) + general_form = c2gconv.conic_to_general_1(output) + e_fitted = B2ACEllipse(*general_form) + assert np.linalg.norm(self.e.center_point - e_fitted.center_point) < 1 + assert np.linalg.norm(max(self.e.radii) - max(e_fitted.radii)) < 0.25 + assert np.linalg.norm(min(self.e.radii) - min(e_fitted.radii)) < 0.25 + assert overlap(self.e, e_fitted) > 0.98 + assert overlap(e_fitted, self.e) > 0.98 diff --git a/tests/test_ext.py b/tests/test_ext.py new file mode 100644 index 0000000..9b7b94f --- /dev/null +++ b/tests/test_ext.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +:mod:`test_ext` +================== + +.. module:: test_ext + :platform: Unix, Windows + :synopsis: + +.. moduleauthor:: hbldh + +Created on 2015-10-13 + +""" + +from __future__ import division +from __future__ import print_function +from __future__ import unicode_literals +from __future__ import absolute_import + + +from __future__ import division +from __future__ import print_function +from __future__ import unicode_literals +from __future__ import absolute_import + +import numpy as np +import b2ac.ext.ellipse_fitter as fitext + +from b2ac.geometry.ellipse import B2ACEllipse +from b2ac.geometry.overlap.overlap_functions import overlap + + +# Bengt Sändh FInn Zetterholm + + +class TestExtensions(object): + + def setup(self): + self.e = B2ACEllipse(center=(50.0, 75.0), radii=(50.0, 20.0), rotation_angle=0.707) + self.points = np.array(self.e.polygonize(), 'int32') + + def test_fit_ext_double_version(self): + output = fitext.fit_ellipse_double(self.points.copy()) + e_fitted = B2ACEllipse(*output) + assert np.linalg.norm(self.e.center_point - e_fitted.center_point) < 1 + assert np.linalg.norm(max(self.e.radii) - max(e_fitted.radii)) < 0.25 + assert np.linalg.norm(min(self.e.radii) - min(e_fitted.radii)) < 0.25 + assert overlap(self.e, e_fitted) > 0.98 + assert overlap(e_fitted, self.e) > 0.98 + + def test_fit_ext_float_version(self): + output = fitext.fit_ellipse_float(self.points.copy()) + e_fitted = B2ACEllipse(*output) + assert np.linalg.norm(self.e.center_point - e_fitted.center_point) < 1 + assert np.linalg.norm(max(self.e.radii) - max(e_fitted.radii)) < 0.25 + assert np.linalg.norm(min(self.e.radii) - min(e_fitted.radii)) < 0.25 + assert overlap(self.e, e_fitted) > 0.98 + assert overlap(e_fitted, self.e) > 0.98 + + def test_fit_ext_int_version(self): + output = fitext.fit_ellipse_int(self.points.copy()) + e_fitted = B2ACEllipse(*output) + assert np.linalg.norm(self.e.center_point - e_fitted.center_point) < 1 + assert np.linalg.norm(max(self.e.radii) - max(e_fitted.radii)) < 0.25 + assert np.linalg.norm(min(self.e.radii) - min(e_fitted.radii)) < 0.25 + assert overlap(self.e, e_fitted) > 0.98 + assert overlap(e_fitted, self.e) > 0.98 diff --git a/tests/test_int.py b/tests/test_int.py new file mode 100644 index 0000000..f527202 --- /dev/null +++ b/tests/test_int.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +:mod:`test_ellipse_fit_reference` +================== + +.. module:: test_ellipse_fit_reference + :platform: Unix, Windows + :synopsis: + +.. moduleauthor:: hbldh + +Created on 2015-10-02 + +""" + +from __future__ import division +from __future__ import print_function +from __future__ import unicode_literals +from __future__ import absolute_import + +import numpy as np +import b2ac.ext.ellipse_fitter as fitext + +from b2ac.fit import fit_improved_B2AC_int +import b2ac.conversion as c2gconv +from b2ac.preprocess import remove_mean_values +from b2ac.geometry.ellipse import B2ACEllipse +from b2ac.geometry.overlap.overlap_functions import overlap + + +class TestIntegerImplementations(object): + + def setup(self): + self.e = B2ACEllipse(center=(50.0, 75.0), radii=(50.0, 20.0), rotation_angle=0.707) + self.points = np.array(self.e.polygonize(), 'int32') + + def test_fit_int_version_1(self): + output = fit_improved_B2AC_int(self.points.copy()) + ellipse_data = c2gconv.conic_to_general_int(output, return_float=True, verbose=True) + e_fitted = B2ACEllipse(*ellipse_data) + assert np.linalg.norm(self.e.center_point - e_fitted.center_point) < 1 + assert np.linalg.norm(max(self.e.radii) - max(e_fitted.radii)) < 1 + assert np.linalg.norm(min(self.e.radii) - min(e_fitted.radii)) < 1 + assert overlap(self.e, e_fitted) > 0.95 + assert overlap(e_fitted, self.e) > 0.95 + + def test_fit_int_version_2(self): + points, x_mean, y_mean = remove_mean_values(self.points.copy()) + output = fit_improved_B2AC_int(points) + ellipse_data = c2gconv.conic_to_general_int(output, return_float=True, verbose=True) + e_fitted = B2ACEllipse(*ellipse_data) + e_fitted.center_point += (x_mean, y_mean) + print(e_fitted) + assert np.linalg.norm(self.e.center_point - e_fitted.center_point) < 1.0 + assert np.linalg.norm(max(self.e.radii) - max(e_fitted.radii)) < 0.1 + assert np.linalg.norm(min(self.e.radii) - min(e_fitted.radii)) < 0.1 + assert overlap(self.e, e_fitted) > 0.98 + assert overlap(e_fitted, self.e) > 0.98 + + def test_fit_ext_int_version(self): + output = fitext.fit_ellipse_int(self.points.copy()) + e_fitted = B2ACEllipse(*output) + assert np.linalg.norm(self.e.center_point - e_fitted.center_point) < 1 + assert np.linalg.norm(max(self.e.radii) - max(e_fitted.radii)) < 0.25 + assert np.linalg.norm(min(self.e.radii) - min(e_fitted.radii)) < 0.25 + assert overlap(self.e, e_fitted) > 0.98 + assert overlap(e_fitted, self.e) > 0.98 + + def test_py_and_ext_similarity(self): + output = fitext.fit_ellipse_int(self.points.copy()) + e_ext = B2ACEllipse(*output) + points, x_mean, y_mean = remove_mean_values(self.points.copy()) + output = fit_improved_B2AC_int(points) + ellipse_data = c2gconv.conic_to_general_int(output, return_float=True, verbose=True) + e_py = B2ACEllipse(*ellipse_data) + e_py.center_point += (x_mean, y_mean) + assert overlap(e_ext, e_py) > 0.99 + assert overlap(e_py, e_ext) > 0.99