boofcv.alg.fiducial.calib.circle.EllipseClustersIntoGrid Maven / Gradle / Ivy

Go to download
/*
 * Copyright (c) 2021, Peter Abeles. All Rights Reserved.
 *
 * This file is part of BoofCV (http://boofcv.org).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package boofcv.alg.fiducial.calib.circle;

import boofcv.alg.fiducial.calib.circle.EllipsesIntoClusters.Node;
import georegression.metric.UtilAngle;
import georegression.struct.curve.EllipseRotated_F64;
import georegression.struct.point.Point2D_F64;
import org.ddogleg.sorting.QuickSortComparator;
import org.ddogleg.struct.DogArray;
import org.ddogleg.struct.FastArray;
import org.jetbrains.annotations.Nullable;

import java.util.ArrayList;
import java.util.List;
import java.util.Objects;

/**
 * Base class for ordering clusters of ellipses into grids
 *
 * See {@link Grid} for a description of how the output grids are described. It uses a sparse format.
 * See {@link DetectCircleHexagonalGrid} for an example of an hexagonal grid
 *
 * @author Peter Abeles
 */
public abstract class EllipseClustersIntoGrid {

	protected DogArray foundGrids = new DogArray<>(Grid::new);

	// When finding lines this is the largest change in angle between the two edges allowed for it to be on the line
	protected static double MAX_LINE_ANGLE_CHANGE = UtilAngle.degreeToRadian(30);

	// Information on each ellipse/node in a cluster
	protected DogArray listInfo = new DogArray<>(NodeInfo::new);
	// Used to sort edges in a node. used instead of built in sorting algorithm to maximize memory being recycled
	protected QuickSortComparator sorter;

	// All ellipses in the contour around the grid
	protected FastArray contour = new FastArray<>(NodeInfo.class);

	protected boolean verbose = false;

	protected EllipseClustersIntoGrid() {
		sorter = new QuickSortComparator<>(( o1, o2 ) -> {
			if (o1.angle < o2.angle)
				return -1;
			else if (o1.angle > o2.angle)
				return 1;
			else
				return 0;
		});
	}

	/**
	 * Computes grids from the clusters. Call {@link #getGrids()} to retrieve the results.
	 *
	 * @param ellipses (input) List of all the ellipses
	 * @param clusters (Input) Description of all the clusters
	 */
	public abstract void process( List ellipses, List> clusters );

	/**
	 * Finds all the nodes which form an approximate line
	 *
	 * @param seed First ellipse
	 * @param next Second ellipse, specified direction of line relative to seed
	 * @return All the nodes along the line
	 */
	protected static @Nullable List findLine( NodeInfo seed,
														@Nullable NodeInfo next, int clusterSize,
														@Nullable List line, boolean ccw ) {
		if (next == null)
			return null;
		if (line == null)
			line = new ArrayList<>();
		else
			line.clear();

		next.marked = true;
		double anglePrev = direction(next, seed);

		double prevDist = next.ellipse.center.distance(seed.ellipse.center);

		line.add(seed);
		line.add(next);

		NodeInfo previous = seed;
		for (int i = 0; i < clusterSize + 1; i++) {
			// find the child of next which is within tolerance and closest to it
			double bestScore = Double.MAX_VALUE;
			double bestDistance = Double.MAX_VALUE;
			double bestAngle = Double.NaN;
			double closestDistance = Double.MAX_VALUE;
			NodeInfo best = null;

			double previousLength = next.ellipse.center.distance(previous.ellipse.center);

//			System.out.println("---- line connecting "+i);
			for (int j = 0; j < next.edges.size(); j++) {

				double angle = next.edges.get(j).angle;
				NodeInfo c = next.edges.get(j).target;
				if (c.marked)
					continue;

				double candidateLength = next.ellipse.center.distance(c.ellipse.center);

				double ratioLengths = previousLength/candidateLength;
				double ratioSize = previous.ellipse.a/c.ellipse.a;
				if (ratioLengths > 1) {
					ratioLengths = 1.0/ratioLengths;
					ratioSize = 1.0/ratioSize;
				}
				if (Math.abs(ratioLengths - ratioSize) > 0.4)
					continue;


				double angleDist = ccw ? UtilAngle.distanceCCW(anglePrev, angle) : UtilAngle.distanceCW(anglePrev, angle);

				if (angleDist <= Math.PI + MAX_LINE_ANGLE_CHANGE) {
					double d = c.ellipse.center.distance(next.ellipse.center);

					double score = d/prevDist + angleDist;

					if (score < bestScore) {
//						System.out.println("  ratios: "+ratioLengths+" "+ratioSize);
						bestDistance = d;
						bestScore = score;
						bestAngle = angle;
						best = c;
					}
					closestDistance = Math.min(d, closestDistance);
				}
			}

			if (best == null || bestDistance > closestDistance*2.0) {
				return line;
			} else {
				best.marked = true;
				prevDist = bestDistance;
				line.add(best);
				anglePrev = UtilAngle.bound(bestAngle + Math.PI);
				previous = next;
				next = best;
			}
		}
//		if( verbose ) {
//			System.out.println("Stuck in a loop?  Maximum line length exceeded");
//		}
		return null;
	}

	/**
	 * Select the first node (currentSeed) in the next row it finds the next element in the next row by
	 * looking at the first and second elements in the previous row. It selects the edge in
	 * currentSeed which cones closest to matching the angle of 'prevSeed' and 'prevNext'
	 *
	 * @param prevSeed First node in the previous row
	 * @param prevNext Second node in the previous row
	 * @param currentSeed First node in the current row
	 * @return The found node or null if one was not found
	 */
	static protected @Nullable NodeInfo selectSeedNext( NodeInfo prevSeed, NodeInfo prevNext,
											  NodeInfo currentSeed, boolean ccw ) {
		double referenceAngle = direction(prevNext, prevSeed);

		double bestScore = Double.MAX_VALUE;
		NodeInfo best = null;

		// cut down on verbosity by saving the reference here
		Point2D_F64 c = currentSeed.ellipse.center;

		for (int i = 0; i < currentSeed.edges.size(); i++) {
			Edge edge = currentSeed.edges.get(i);
			if (edge.target.marked)
				continue;

			double angle = edge.angle;
			double angleDist = ccw ? UtilAngle.distanceCCW(referenceAngle, angle) : UtilAngle.distanceCW(referenceAngle, angle);
			if (angleDist > Math.PI + MAX_LINE_ANGLE_CHANGE)
				continue;

			Point2D_F64 p = edge.target.ellipse.center;

			double score = angleDist*c.distance(p);

			if (score < bestScore) {
				bestScore = score;
				best = edge.target;
			}
		}

		if (best != null)
			best.marked = true;
		return best;
	}

	/**
	 * Finds the node which is an edge of 'n' that is closest to point 'p'
	 */
	protected static @Nullable NodeInfo findClosestEdge( NodeInfo n, Point2D_F64 p ) {
		double bestDistance = Double.MAX_VALUE;
		NodeInfo best = null;

		for (int i = 0; i < n.edges.size(); i++) {
			Edge e = n.edges.get(i);
			if (e.target.marked)
				continue;

			double d = e.target.ellipse.center.distance2(p);
			if (d < bestDistance) {
				bestDistance = d;
				best = e.target;
			}
		}

		return best;
	}

	/**
	 * Checks to see if any node is used more than once
	 */
	boolean checkDuplicates( List> grid ) {

		for (int i = 0; i < listInfo.size; i++) {
			listInfo.get(i).marked = false;
		}

		for (int i = 0; i < grid.size(); i++) {
			List list = grid.get(i);
			for (int j = 0; j < list.size(); j++) {
				NodeInfo n = list.get(j);
				if (n.marked)
					return true;
				n.marked = true;
			}
		}
		return false;
	}

	static double direction( NodeInfo src, NodeInfo dst ) {
		return Math.atan2(dst.ellipse.center.y - src.ellipse.center.y,
				dst.ellipse.center.x - src.ellipse.center.x);
	}

	/**
	 * For each cluster create a {@link NodeInfo} and compute different properties
	 */
	void computeNodeInfo( List ellipses, List cluster ) {

		// create an info object for each member inside of the cluster
		listInfo.reset();
		for (int i = 0; i < cluster.size(); i++) {
			Node n = cluster.get(i);
			EllipseRotated_F64 t = ellipses.get(n.which);

			NodeInfo info = listInfo.grow();
			info.reset();
			info.ellipse = t;
		}

		addEdgesToInfo(cluster);
		pruneNearlyIdenticalAngles();
		findLargestAnglesForAllNodes();
	}

	/**
	 * Adds edges to node info and computes their orientation
	 */
	void addEdgesToInfo( List cluster ) {
		for (int i = 0; i < cluster.size(); i++) {
			Node n = cluster.get(i);
			NodeInfo infoA = listInfo.get(i);
			EllipseRotated_F64 a = infoA.ellipse;

			// create the edges and order them based on their direction
			for (int j = 0; j < n.connections.size(); j++) {
				NodeInfo infoB = listInfo.get(indexOf(cluster, n.connections.get(j)));

				EllipseRotated_F64 b = infoB.ellipse;

				Edge edge = infoA.edges.grow();

				edge.target = infoB;
				edge.angle = Math.atan2(b.center.y - a.center.y, b.center.x - a.center.x);
			}

			sorter.sort(infoA.edges.data, infoA.edges.size);
		}
	}

	/**
	 * If there is a nearly perfect line a node farther down the line can come before. This just selects the closest
	 */
	void pruneNearlyIdenticalAngles() {
		for (int i = 0; i < listInfo.size(); i++) {
			NodeInfo infoN = listInfo.get(i);

			for (int j = 0; j < infoN.edges.size(); ) {
				int k = (j + 1)%infoN.edges.size;

				double angularDiff = UtilAngle.dist(infoN.edges.get(j).angle, infoN.edges.get(k).angle);
				if (angularDiff < UtilAngle.radian(5)) {
					NodeInfo infoJ = infoN.edges.get(j).target;
					NodeInfo infoK = infoN.edges.get(k).target;

					double distJ = infoN.ellipse.center.distance(infoJ.ellipse.center);
					double distK = infoN.ellipse.center.distance(infoK.ellipse.center);

					if (distJ < distK) {
						infoN.edges.remove(k);
					} else {
						infoN.edges.remove(j);
					}
				} else {
					j++;
				}
			}
		}
	}

	/**
	 * Finds the two edges with the greatest angular distance between them.
	 */
	void findLargestAnglesForAllNodes() {
		for (int i = 0; i < listInfo.size(); i++) {
			NodeInfo info = listInfo.get(i);

			if (info.edges.size < 2)
				continue;

			for (int k = 0, j = info.edges.size - 1; k < info.edges.size; j = k, k++) {
				double angleA = info.edges.get(j).angle;
				double angleB = info.edges.get(k).angle;

				double distance = UtilAngle.distanceCCW(angleA, angleB);

				if (distance > info.angleBetween) {
					info.angleBetween = distance;
					info.left = info.edges.get(j).target;
					info.right = info.edges.get(k).target;
				}
			}
		}
	}

	/**
	 * Finds nodes in the outside of the grid. First the node in the grid with the largest 'angleBetween'
	 * is selected as a seed. It is assumed at this node must be on the contour. Then the graph is traversed
	 * in CCW direction until a loop is formed.
	 *
	 * @return true if valid and false if invalid
	 */
	boolean findContour( boolean mustHaveInner ) {
		// find the node with the largest angleBetween
		NodeInfo seed = listInfo.get(0);
		for (int i = 1; i < listInfo.size(); i++) {
			NodeInfo info = listInfo.get(i);

			if (info.angleBetween > seed.angleBetween) {
				seed = info;
			}
		}

		// trace around the contour
		contour.reset();
		contour.add(seed);
		seed.contour = true;
		NodeInfo prev = seed;
		NodeInfo current = seed.right;
		while (current != null && current != seed && contour.size() < listInfo.size()) {
			if (prev != current.left)
				return false;
			contour.add(current);
			current.contour = true;
			prev = current;
			current = current.right;
		}

		// fail if it is too small or was cycling
		return !(contour.size < 4 || (mustHaveInner && contour.size >= listInfo.size()));
	}

	/**
	 * Finds the node with the index of 'value'
	 */
	public static int indexOf( List list, int value ) {

		for (int i = 0; i < list.size(); i++) {
			if (list.get(i).which == value)
				return i;
		}

		return -1;
	}

	/**
	 * Pick the node in the contour with the largest angle. Distortion tends to make the acute angle smaller.
	 * Without distortion it will be 270 degrees.
	 */
	NodeInfo selectSeedCorner() {
		NodeInfo best = null;
		double bestAngle = 0;

		for (int i = 0; i < contour.size; i++) {
			NodeInfo info = contour.get(i);

			if (info.angleBetween > bestAngle) {
				bestAngle = info.angleBetween;
				best = info;
			}
		}

		Objects.requireNonNull(best).marked = true;
		return best;
	}

	/**
	 * Returns the set of grids which were found
	 *
	 * @return found grids
	 */
	public DogArray getGrids() {
		return foundGrids;
	}

	@SuppressWarnings({"NullAway.Init"})
	public static class NodeInfo {
		EllipseRotated_F64 ellipse;

		// List of all the ellipses connected to this one in CCW order
		DogArray edges = new DogArray<>(Edge::new);

		// flag used to indicate if a node is along the shape's contour
		boolean contour;
		// the largest angle between two nodes is angleBetween and
		// left is before right in CCW direction
		NodeInfo left, right;
		double angleBetween;

		// used to indicate if it has been inspected already
		boolean marked;

		public @Nullable Edge findEdge( NodeInfo target ) {
			for (int i = 0; i < edges.size; i++) {
				if (edges.get(i).target == target) {
					return edges.get(i);
				}
			}
			return null;
		}

		public double distance( NodeInfo target ) {
			return ellipse.center.distance(target.ellipse.center);
		}

		@SuppressWarnings("NullAway")
		public void reset() {
			contour = false;
			ellipse = null;
			left = right = null;
			angleBetween = 0;
			marked = false;
			edges.reset();
		}
	}

	@SuppressWarnings({"NullAway.Init"})
	public static class Edge {
		NodeInfo target;
		double angle;

		public Edge() {}

		public Edge( NodeInfo target, double angle ) {
			this.target = target;
			this.angle = angle;
		}
	}

	public boolean isVerbose() {
		return verbose;
	}

	public void setVerbose( boolean verbose ) {
		this.verbose = verbose;
	}

	/**
	 * Specifies the grid. See implementation class for grid details.
	 */
	public static class Grid {
		public List ellipses = new ArrayList<>();
		public int rows;
		public int columns;

		public void reset() {
			rows = columns = -1;
			ellipses.clear();
		}

		public EllipseRotated_F64 get( int row, int col ) {
			return ellipses.get(row*columns + col);
		}

		public int idx( int row, int col ) {
			return row*columns + col;
		}

		public void setShape( int rows, int columns ) {
			this.rows = rows;
			this.columns = columns;
		}

		public int getIndexOfHexEllipse( int row, int col ) {
			int index = 0;
			index += (row/2)*this.columns + (row%2)*(this.columns/2 + this.columns%2);
			return index + col/2;
		}

		public int getIndexOfRegEllipse( int row, int col ) {
			return row*this.columns + col;
		}
	}
}