001/*******************************************************************************
002 * This software is provided as a supplement to the authors' textbooks on digital
003 * image processing published by Springer-Verlag in various languages and editions.
004 * Permission to use and distribute this software is granted under the BSD 2-Clause
005 * "Simplified" License (see http://opensource.org/licenses/BSD-2-Clause).
006 * Copyright (c) 2006-2023 Wilhelm Burger, Mark J. Burge. All rights reserved.
007 * Visit https://imagingbook.com for additional details.
008 ******************************************************************************/
009package imagingbook.common.geometry.fitting.line;
010
011import imagingbook.common.geometry.basic.Pnt2d;
012import imagingbook.common.math.eigen.EigenDecompositionJama;
013import imagingbook.common.util.PrimitiveSortMap;
014import org.apache.commons.math3.linear.MatrixUtils;
015
016import java.util.ArrayDeque;
017import java.util.Deque;
018
019import static imagingbook.common.math.Arithmetic.sqr;
020
021/**
022 * <p>
023 * This class implements incremental orthogonal line fitting to a set of 2D points using eigendecomposition (see
024 * {@link OrthogonalLineFitEigen} for a non-incremental version). See Sec. 10.3 (Alg. 10.4) of [1] for additional
025 * details.
026 * </p>
027 * <p>
028 * This fitter behaves like a queue: sample points may be added and removed freely either at its front or its end, while
029 * the ordering of the remaining points remains unchanged. This is to simplify back-tracking, for example for
030 * incremental contour fitting. Whenever a point is added or removed, the internal statistics (scatter matrix) are
031 * updated. The current line fit can be queried any time as long as there are more than two points in the point set
032 * (otherwise an exception is thrown).
033 * </p>
034 * <p>
035 * [1] W. Burger, M.J. Burge, <em>Digital Image Processing &ndash; An Algorithmic Introduction</em>, 3rd ed, Springer
036 * (2022).
037 * </p>
038 *
039 * @author WB
040 * @version 2022/09/29 revised
041 * @see OrthogonalLineFitEigen
042 * @see Deque
043 */
044public class IncrementalLineFit implements LineFit {    // extends ArrayDeque<Pnt2d> 
045        
046        private final Deque<Pnt2d> points;
047        private double[] p;                                                                     // current line parameters A,B,C
048        private double Sx, Sy, Sxx, Syy, Sxy;   // current scatter statistics
049        
050        /**
051         * Constructor creating a fitter with an empty set of points.
052         */
053        public IncrementalLineFit() {
054                this(null);
055        }
056
057        /**
058         * Constructor accepting a sequence of initial points. The first point is placed at the from of the queue, the last
059         * point at its end.
060         *
061         * @param initPnts an array of initial points
062         */
063        public IncrementalLineFit(Pnt2d[] initPnts) {   
064                points = new ArrayDeque<>();
065                p = null;
066                Sx = 0;
067                Sy = 0;
068                Sxx = 0;
069                Syy = 0;
070                Sxy = 0;
071                if (initPnts == null) {
072                        return;
073                }
074                for (Pnt2d p : initPnts) {
075                        this.add(p);
076                }
077        }
078
079        /**
080         * Calculates and returns the sum of the squared orthogonal distances of the current point set for this line fit.
081         *
082         * @return the squared orthogonal error
083         * @see LineFit#getSquaredOrthogonalError(Pnt2d[])
084         */
085        public double getSquaredOrthogonalError() {
086                return getSquaredOrthogonalError(this.getPoints());
087        }
088        
089        // update statistics at point addition/removal --------------------- 
090        
091        private void addSamplePoint(Pnt2d pnt) {
092                final double x = pnt.getX();
093                final double y = pnt.getY();
094                Sx += x;
095                Sy += y;
096                Sxx += sqr(x);
097                Syy += sqr(y);
098                Sxy += x * y;
099                this.p = null;  // invalidate previous line parameters
100        }
101        
102        private void removeSamplePoint(Pnt2d pnt) {
103                final double x = pnt.getX();
104                final double y = pnt.getY();
105                Sx -= x;
106                Sy -= y;
107                Sxx -= sqr(x);
108                Syy -= sqr(y);
109                Sxy -= x * y;
110                this.p = null;  // invalidate previous line parameters
111        }
112        
113        // --------------------------------------------------------
114        
115        /**
116         * Returns the current point sequence of this fit as an array.
117         * @return an array of points
118         */
119        public Pnt2d[] getPoints() {
120                return points.toArray(new Pnt2d[0]);
121        }
122        
123        // delegate methods matching those of Deque interface
124
125        /**
126         * Adds a new sample points to the end of the point sequence (same as {@link #addLast(Pnt2d)}).
127         *
128         * @param pnt the point to be added
129         * @return true (always, to be compatible with {@link Deque})
130         */
131        public boolean add(Pnt2d pnt) {
132                addLast(pnt);
133                return true;
134        }
135        
136        /**
137         * Adds a new sample points to the front of the point sequence.
138         * @param pnt the point to be added
139         */
140        public void addFirst(Pnt2d pnt) {
141                points.addFirst(pnt);
142                addSamplePoint(pnt);
143        }
144
145        /**
146         * Adds a new sample points to the end of the point sequence (same as {@link #addLast(Pnt2d)}).
147         *
148         * @param pnt the point to be added
149         */
150        public void addLast(Pnt2d pnt) {
151                points.addLast(pnt);
152                addSamplePoint(pnt);
153        }
154        
155        /**
156         * Retrieves and removes the first point from the front of the point sequence.
157         * @return the removed point
158         */
159        public Pnt2d removeFirst() {
160                Pnt2d pnt = points.removeFirst();
161                removeSamplePoint(pnt);
162                return pnt;
163        }
164        
165        /**
166         * Retrieves and removes the last point from the end of the point sequence.
167         * @return the removed point
168         */
169        public Pnt2d removeLast() {
170                Pnt2d pnt = points.removeLast();
171                removeSamplePoint(pnt);
172                return pnt;
173        }
174
175        /**
176         * Returns (but does not remove) the point positioned at the front of the point sequence.
177         *
178         * @return the first point
179         */
180        public Pnt2d peekFirst() {
181                return points.peekFirst();
182        }
183
184        /**
185         * Returns (but does not remove) the point positioned at the end of the point sequence.
186         *
187         * @return the last point
188         */
189        public Pnt2d peekLast() {
190                return points.peekLast();
191        }
192        
193        // --------------------------------------------------------
194        
195        @Override
196        public int getSize() {
197                return points.size();
198        }
199        
200        @Override
201        public double[] getLineParameters() {
202                if (this.getSize() < 2) {
203                        throw new IllegalStateException("cannot fit line, set of point set is less than 2");
204                }
205                if (p == null) {
206                        this.p = fit(points.toArray(new Pnt2d[0]));
207                }
208                return p;
209        }
210        
211        private double[] fit(Pnt2d[] pts) {
212                final int n = pts.length;
213                
214                double sxx = Sxx - sqr(Sx) / n;
215                double syy = Syy - sqr(Sy) / n;
216                double sxy = Sxy - Sx * Sy / n;
217                
218                double[][] S = {
219                                {sxx, sxy},
220                                {sxy, syy}};
221                
222                EigenDecompositionJama es = new EigenDecompositionJama(MatrixUtils.createRealMatrix(S));
223                int k = PrimitiveSortMap.getNthSmallestIndex(es.getRealEigenvalues(), 0);
224                double[] e = es.getEigenvector(k).toArray();
225                
226                double A = e[0];
227                double B = e[1];
228                double C = -(A * Sx + B * Sy) / n;
229                
230                return new double[] {A, B, C};
231        }
232                
233}