boofcv.alg.fiducial.calib.squares.SquareGridTools Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of boofcv-recognition Show documentation
BoofCV is an open source Java library for real-time computer vision and robotics applications.
The newest version!
/*
 * 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.squares;

import georegression.geometry.UtilLine2D_F64;
import georegression.metric.Area2D_F64;
import georegression.metric.Intersection2D_F64;
import georegression.struct.line.LineGeneral2D_F64;
import georegression.struct.line.LineSegment2D_F64;
import georegression.struct.point.Point2D_F64;
import georegression.struct.shapes.Polygon2D_F64;

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

/**
 * A class for manipulating {@link SquareGrid}
 *
 * @author Peter Abeles
 */
public class SquareGridTools {
	/**
	 * There can be 2 or 4 possible orientations which are equally valid solutions. For
	 * sake of consistency it will make the (0,0) coordinate be closest to the origin
	 * of the image coordinate system.
	 */
	public void putIntoCanonical( SquareGrid grid ) {
		if (grid.rows == grid.columns) {
			int best = -1;
			double bestDistance = Double.MAX_VALUE;
			for (int i = 0; i < 4; i++) {
				SquareNode n = grid.getCornerByIndex(i);
				double d = n.center.normSq();
				if (d < bestDistance) {
					best = i;
					bestDistance = d;
				}
			}

			for (int i = 0; i < best; i++) {
				rotateCCW(grid);
			}
		} else {
			double first = grid.get(0, 0).center.normSq();
			double last = grid.getCornerByIndex(2).center.normSq();

			if (last < first) {
				reverse(grid);
			}
		}
	}

	public void rotateCCW( SquareGrid grid ) {
		tmp.clear();
		for (int row = 0; row < grid.rows; row++) {
			for (int col = 0; col < grid.columns; col++) {
				tmp.add(grid.get(col, grid.columns - row - 1));
			}
		}
		grid.nodes.clear();
		grid.nodes.addAll(tmp);
	}

	public void reverse( SquareGrid grid ) {
		tmp.clear();
		int N = grid.columns*grid.rows;
		for (int i = 0; i < N; i++) {
			tmp.add(grid.nodes.get(N - i - 1));
		}
		grid.nodes.clear();
		grid.nodes.addAll(tmp);
	}

	/**
	 * Checks to see if it needs to be flipped. Flipping is required if X and Y axis in 2D grid
	 * are not CCW.
	 */
	public boolean checkFlip( SquareGrid grid ) {
		if (grid.columns == 1 || grid.rows == 1)
			return false;

		Point2D_F64 a = grid.get(0, 0).center;
		Point2D_F64 b = grid.get(0, grid.columns - 1).center;
		Point2D_F64 c = grid.get(grid.rows - 1, 0).center;

		double x0 = b.x - a.x;
		double y0 = b.y - a.y;

		double x1 = c.x - a.x;
		double y1 = c.y - a.y;

		double z = x0*y1 - y0*x1;

		return z < 0;
	}

	/**
	 * Compute the visual size of a polygon
	 */
	Polygon2D_F64 poly = new Polygon2D_F64(4);

	public double computeSize( SquareGrid grid ) {
		poly.vertexes.data[0] = grid.get(0, 0).center;
		poly.vertexes.data[1] = grid.get(0, grid.columns - 1).center;
		poly.vertexes.data[2] = grid.get(grid.rows - 1, grid.columns - 1).center;
		poly.vertexes.data[3] = grid.get(grid.rows - 1, 0).center;

		return Area2D_F64.polygonSimple(poly);
	}

	List tmp = new ArrayList<>();

	/**
	 * Transposes the grid
	 */
	public void transpose( SquareGrid grid ) {
		tmp.clear();

		for (int col = 0; col < grid.columns; col++) {
			for (int row = 0; row < grid.rows; row++) {
				tmp.add(grid.get(row, col));
			}
		}

		grid.nodes.clear();
		grid.nodes.addAll(tmp);

		int a = grid.columns;
		grid.columns = grid.rows;
		grid.rows = a;
	}

	/**
	 * Flips the order of rows
	 */
	public void flipRows( SquareGrid grid ) {
		tmp.clear();

		for (int row = 0; row < grid.rows; row++) {
			for (int col = 0; col < grid.columns; col++) {
				tmp.add(grid.nodes.get((grid.rows - row - 1)*grid.columns + col));
			}
		}

		grid.nodes.clear();
		grid.nodes.addAll(tmp);
	}

	/**
	 * Flips the order of columns
	 */
	public void flipColumns( SquareGrid grid ) {
		tmp.clear();

		for (int row = 0; row < grid.rows; row++) {
			for (int col = 0; col < grid.columns; col++) {
				tmp.add(grid.get(row, grid.columns - col - 1));
			}
		}

		grid.nodes.clear();
		grid.nodes.addAll(tmp);
	}

	/**
	 * Get outside corner polygon around the grid. The grid is assumed to be in CCW orientation.
	 */
	public void boundingPolygonCCW( SquareGrid grid, Polygon2D_F64 bounding ) {
		int w = grid.columns;
		int h = grid.rows;

		if (w == 1 && h == 1) {
			SquareNode n = grid.get(0, 0);
			bounding.get(0).setTo(n.square.get(0));
			bounding.get(1).setTo(n.square.get(1));
			bounding.get(2).setTo(n.square.get(2));
			bounding.get(3).setTo(n.square.get(3));
		} else if (w == 1) {
			orderNode(grid.get(0, 0), grid.get(h - 1, 0), false);
			bounding.get(0).setTo(ordered[0]);
			bounding.get(1).setTo(ordered[1]);
			orderNode(grid.get(h - 1, 0), grid.get(0, 0), false);
			bounding.get(2).setTo(ordered[0]);
			bounding.get(3).setTo(ordered[1]);
		} else if (h == 1) {
			orderNode(grid.get(0, 0), grid.get(0, w - 1), true);
			bounding.get(0).setTo(ordered[0]);
			bounding.get(3).setTo(ordered[3]);
			orderNode(grid.get(0, w - 1), grid.get(0, 0), true);
			bounding.get(1).setTo(ordered[3]);
			bounding.get(2).setTo(ordered[0]);
		} else {
			orderNode(grid.get(0, 0), grid.get(0, w - 1), true);
			bounding.get(0).setTo(ordered[0]);
			orderNode(grid.get(0, w - 1), grid.get(h - 1, w - 1), true);
			bounding.get(1).setTo(ordered[0]);
			orderNode(grid.get(h - 1, w - 1), grid.get(h - 1, 0), true);
			bounding.get(2).setTo(ordered[0]);
			orderNode(grid.get(h - 1, 0), grid.get(0, 0), true);
			bounding.get(3).setTo(ordered[0]);
		}
	}

	/**
	 * Given the grid coordinate, order the corners for the node at that location. Takes in handles situations
	 * where there are no neighbors.
	 */
	protected void orderNodeGrid( SquareGrid grid, int row, int col ) {
		SquareNode node = grid.get(row, col);

		if (grid.rows == 1 && grid.columns == 1) {
			for (int i = 0; i < 4; i++) {
				ordered[i] = node.square.get(i);
			}
		} else if (grid.columns == 1) {
			if (row == grid.rows - 1) {
				orderNode(node, grid.get(row - 1, col), false);
				rotateTwiceOrdered();
			} else {
				orderNode(node, grid.get(row + 1, col), false);
			}
		} else {
			if (col == grid.columns - 1) {
				orderNode(node, grid.get(row, col - 1), true);
				rotateTwiceOrdered();
			} else {
				orderNode(node, grid.get(row, col + 1), true);
			}
		}
	}

	/**
	 * Reorders the list by the equivalent of two rotations
	 */
	private void rotateTwiceOrdered() {
		Point2D_F64 a = ordered[0];
		Point2D_F64 b = ordered[1];
		Point2D_F64 c = ordered[2];
		Point2D_F64 d = ordered[3];

		ordered[0] = c;
		ordered[1] = d;
		ordered[2] = a;
		ordered[3] = b;
	}

	LineSegment2D_F64 lineCenters = new LineSegment2D_F64();
	LineSegment2D_F64 lineSide = new LineSegment2D_F64();
	Point2D_F64 dummy = new Point2D_F64();

	LineGeneral2D_F64 general = new LineGeneral2D_F64();
	Point2D_F64 ordered[] = new Point2D_F64[4];

	/**
	 * Fills the ordered list with the corners in target node in canonical order.
	 *
	 * @param pointingX true if 'node' is pointing along the x-axis from target. false for point along y-axis
	 */
	protected void orderNode( SquareNode target, SquareNode node, boolean pointingX ) {

		int index0 = findIntersection(target, node);
		int index1 = (index0 + 1)%4;

		int index2 = (index0 + 2)%4;
		int index3 = (index0 + 3)%4;

		if (index0 < 0)
			throw new RuntimeException("Couldn't find intersection. Probable bug");

		lineCenters.a = target.center;
		lineCenters.b = node.center;
		UtilLine2D_F64.convert(lineCenters, general);

		Polygon2D_F64 poly = target.square;
		if (pointingX) {
			if (sign(general, poly.get(index0)) > 0) {
				ordered[1] = poly.get(index1);
				ordered[2] = poly.get(index0);
			} else {
				ordered[1] = poly.get(index0);
				ordered[2] = poly.get(index1);
			}
			if (sign(general, poly.get(index2)) > 0) {
				ordered[3] = poly.get(index2);
				ordered[0] = poly.get(index3);
			} else {
				ordered[3] = poly.get(index3);
				ordered[0] = poly.get(index2);
			}
		} else {
			if (sign(general, poly.get(index0)) > 0) {
				ordered[2] = poly.get(index1);
				ordered[3] = poly.get(index0);
			} else {
				ordered[2] = poly.get(index0);
				ordered[3] = poly.get(index1);
			}
			if (sign(general, poly.get(index2)) > 0) {
				ordered[0] = poly.get(index2);
				ordered[1] = poly.get(index3);
			} else {
				ordered[0] = poly.get(index3);
				ordered[1] = poly.get(index2);
			}
		}
	}

	/**
	 * Finds the side which intersects the line segment from the center of target to center of node
	 */
	protected int findIntersection( SquareNode target, SquareNode node ) {
		lineCenters.a = target.center;
		lineCenters.b = node.center;

		for (int i = 0; i < 4; i++) {
			int j = (i + 1)%4;

			lineSide.a = target.square.get(i);
			lineSide.b = target.square.get(j);

			if (Intersection2D_F64.intersection(lineCenters, lineSide, dummy) != null) {
				return i;
			}
		}
		return -1;
	}

	/**
	 * Adjust the corners in the square's polygon so that they are aligned along the grids overall
	 * length
	 *
	 * @return true if valid grid or false if not
	 */
	public boolean orderSquareCorners( SquareGrid grid ) {

		// the first pass interleaves every other row
		for (int row = 0; row < grid.rows; row++) {

			for (int col = 0; col < grid.columns; col++) {
				orderNodeGrid(grid, row, col);
				Polygon2D_F64 square = grid.get(row, col).square;

				for (int i = 0; i < 4; i++) {
					square.vertexes.data[i] = ordered[i];
				}
			}
		}

		return true;
	}

	public static int sign( LineGeneral2D_F64 line, Point2D_F64 p ) {
		double val = line.A*p.x + line.B*p.y + line.C;
		if (val > 0)
			return 1;
		if (val < 0)
			return -1;
		return 0;
	}
}