boofcv.alg.fiducial.calib.circle.KeyPointsCircleHexagonalGrid 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.circle;

import boofcv.alg.fiducial.calib.circle.EllipseClustersIntoGrid.Grid;
import boofcv.struct.geo.PointIndex2D_F64;
import georegression.geometry.UtilLine2D_F64;
import georegression.geometry.UtilVector2D_F64;
import georegression.geometry.curves.TangentLinesTwoEllipses_F64;
import georegression.metric.Intersection2D_F64;
import georegression.misc.GrlConstants;
import georegression.struct.curve.EllipseRotated_F64;
import georegression.struct.line.LineGeneral2D_F64;
import georegression.struct.point.Point2D_F64;
import org.ddogleg.struct.DogArray;

/**
 * Computes key points from an observed hexagonal circular grid. Each key point is defined as the circle's true
 * geometric center. The center (green dot) is found by detecting tangent points between two neighboring
 * circles (yellow dots) and
 * then finding the intersection of all the lines created by connecting adjacent tangent points.
 * Tangent points are the same under perspective distortion and the same can be said for the
 * intersection of their lines.
 *
 * 
 * 
 * 
 *
 * @author Peter Abeles
 */
public class KeyPointsCircleHexagonalGrid {

	// tangent points on each ellipse
	DogArray tangents = new DogArray<>(Tangents::new);

	// detected location
	DogArray keypoints = new DogArray<>(PointIndex2D_F64::new);

	// used to compute tangent lines between two ellipses
	private final TangentLinesTwoEllipses_F64 tangentFinder = new TangentLinesTwoEllipses_F64(GrlConstants.TEST_F64, 10);

	// storage for tangent points on ellipses
	private final Point2D_F64 A0 = new Point2D_F64();
	private final Point2D_F64 A1 = new Point2D_F64();
	private final Point2D_F64 A2 = new Point2D_F64();
	private final Point2D_F64 A3 = new Point2D_F64();
	private final Point2D_F64 B0 = new Point2D_F64();
	private final Point2D_F64 B1 = new Point2D_F64();
	private final Point2D_F64 B2 = new Point2D_F64();
	private final Point2D_F64 B3 = new Point2D_F64();

	// local work space for center of intersections
	private final LineGeneral2D_F64 lineA = new LineGeneral2D_F64();
	private final LineGeneral2D_F64 lineB = new LineGeneral2D_F64();
	private final Point2D_F64 location = new Point2D_F64();

	/**
	 * Computes key points from the grid of ellipses
	 *
	 * @param grid Grid of ellipses
	 * @return true if successful or false if it failed
	 */
	public boolean process( Grid grid ) {
		// reset and initialize data structures
		init(grid);

		// add tangent points from adjacent ellipses
		if (!horizontal(grid))
			return false;
		if (!vertical(grid))
			return false;
		if (!diagonalLR(grid))
			return false;
		if (!diagonalRL(grid))
			return false;

		return computeEllipseCenters();
	}

	void init( Grid grid ) {
		int totalEllipses = 0;
		for (int i = 0; i < grid.ellipses.size(); i++) {
			if (grid.ellipses.get(i) != null)
				totalEllipses++;
		}

		tangents.resize(totalEllipses);
		for (int i = 0; i < tangents.size(); i++) {
			tangents.get(i).reset();
		}
	}

	boolean horizontal( Grid grid ) {
		for (int i = 0; i < grid.rows; i++) {
			for (int j = 0; j < grid.columns - 2; j++) {
				if (i%2 == 0 && j%2 == 1) continue;
				if (i%2 == 1 && j%2 == 0) continue;

				if (!addTangents(grid, i, j, i, j + 2))
					return false;
			}
		}

		return true;
	}

	boolean vertical( Grid grid ) {
		for (int i = 0; i < grid.rows - 2; i++) {
			for (int j = 0; j < grid.columns; j++) {
				if (i%2 == 0 && j%2 == 1) continue;
				if (i%2 == 1 && j%2 == 0) continue;

				if (!addTangents(grid, i, j, i + 2, j))
					return false;
			}
		}

		return true;
	}

	boolean diagonalLR( Grid grid ) {
		for (int i = 0; i < grid.rows - 1; i++) {
			for (int j = 0; j < grid.columns - 1; j++) {
				if (i%2 == 0 && j%2 == 1) continue;
				if (i%2 == 1 && j%2 == 0) continue;

				if (!addTangents(grid, i, j, i + 1, j + 1))
					return false;
			}
		}

		return true;
	}

	boolean diagonalRL( Grid grid ) {
		for (int i = 0; i < grid.rows - 1; i++) {
			for (int j = 1; j < grid.columns; j++) {
				if (i%2 == 0 && j%2 == 1) continue;
				if (i%2 == 1 && j%2 == 0) continue;

				if (!addTangents(grid, i, j, i + 1, j - 1))
					return false;
			}
		}

		return true;
	}

	/**
	 * Computes tangent points to the two ellipses specified by the grid coordinates
	 */
	private boolean addTangents( Grid grid, int rowA, int colA, int rowB, int colB ) {
		EllipseRotated_F64 a = grid.get(rowA, colA);
		EllipseRotated_F64 b = grid.get(rowB, colB);

		if (a == null || b == null) {
			return false;
		}

		if (!tangentFinder.process(a, b, A0, A1, A2, A3, B0, B1, B2, B3)) {
			return false;
		}
		Tangents ta = tangents.get(grid.getIndexOfHexEllipse(rowA, colA));
		Tangents tb = tangents.get(grid.getIndexOfHexEllipse(rowB, colB));

		// add tangent points from the two lines which do not cross the center line
		ta.grow().setTo(A0);
		ta.grow().setTo(A3);

		tb.grow().setTo(B0);
		tb.grow().setTo(B3);
		return true;
	}

	/**
	 * Finds the intersection of all the tangent lines with each other the computes the average of those points.
	 * That location is where the center is set to. Each intersection of lines is weighted by the acute angle.
	 * lines which are 90 degrees to each other are less sensitive to noise
	 */
	boolean computeEllipseCenters() {
		keypoints.reset();

		for (int tangentIdx = 0; tangentIdx < tangents.size(); tangentIdx++) {
//			System.out.println("tangent id "+tangentIdx);
			Tangents t = tangents.get(tangentIdx);
			PointIndex2D_F64 keyPoint = keypoints.grow();
			keyPoint.setTo(0, 0, tangentIdx);
			Point2D_F64 center = keyPoint.p;
			double totalWeight = 0;

			for (int i = 0; i < t.size(); i += 2) {
				UtilLine2D_F64.convert(t.get(i), t.get(i + 1), lineA);

				for (int j = i + 2; j < t.size(); j += 2) {
					UtilLine2D_F64.convert(t.get(j), t.get(j + 1), lineB);

					// way each intersection based on the acute angle. lines which are nearly parallel will
					// be unstable estimates
					double w = UtilVector2D_F64.acute(lineA.A, lineA.B, lineB.A, lineB.B);
					if (w > Math.PI/2.0)
						w = Math.PI - w;

					// If there is perfect data and no noise there will be duplicated lines. With noise there will
					// be very similar lines
					if (w <= 0.02)
						continue;

					if (null == Intersection2D_F64.intersection(lineA, lineB, location)) {
						return false;
					}

//					System.out.printf("   %4.2f loc %6.2f %6.2f\n",w,location.x,location.y);
					center.x += location.x*w;
					center.y += location.y*w;

					totalWeight += w;
				}
			}

			if (totalWeight == 0)
				return false;

			center.x /= totalWeight;
			center.y /= totalWeight;
		}

		return true;
	}

	/**
	 * Returns the location of each key point in the image from the most recently processed grid.
	 *
	 * @return detected image location
	 */
	public DogArray getKeyPoints() {
		return keypoints;
	}

	public DogArray getTangents() {
		return tangents;
	}

	public static class Tangents extends DogArray {
		public Tangents() {
			super(8, Point2D_F64::new);
		}
	}
}