boofcv.alg.fiducial.calib.circle.EllipseClustersIntoHexagonalGrid 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
Show all versions of boofcv-recognition Show documentation
BoofCV is an open source Java library for real-time computer vision and robotics applications.
/*
* 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 org.jetbrains.annotations.Nullable;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
/**
* Given a cluster of ellipses (created with {@link EllipsesIntoClusters}) order the ellipses into
* a hexagonal grid pattern. In a hexagonal grid the center of each circle is the same distance from
* its neighbors. Smallest grid size it will detect is 3 x 3.
*
*
* Note that the returned grid is 'sparse'. every other node is skipped implicitly.
* This is caused by the asymmetry. Each row is offset by one circle/grid element.
*
* Examples:
* 3x6 grid will have 9 elements total.
* grid(0,0) = [0]
* grid(0,2) = [1]
* grid(0,4) = [2]
* grid(1,1) = [3]
* grid(1,3) = [4]
* grid(1,5) = [5]
*
*
* IMPORTANT: To properly construct the contour the clusters need to connect and jump over the "zig-zags". Thus
* at a minimum search radius should be distance between centers*2 if not more.
*
*
* 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 class EllipseClustersIntoHexagonalGrid extends EllipseClustersIntoGrid {
/**
* {@inheritDoc}
*/
@Override
public void process( List ellipses, List> clusters ) {
foundGrids.reset();
if (clusters.size() == 0)
return;
for (int i = 0; i < clusters.size(); i++) {
List cluster = clusters.get(i);
int clusterSize = cluster.size();
if (clusterSize < 6) // 3 x 4 grid has 6 elements
continue;
computeNodeInfo(ellipses, cluster);
// finds all the nodes in the outside of the cluster
if (!findContour(true)) {
if (verbose) System.out.println("Contour find failed");
continue;
}
// Find corner to start alignment
NodeInfo corner = selectSeedCorner();
if (corner == null) {
if (verbose) System.out.println("No corner found!");
continue;
}
List> grid = new ArrayList<>();
// traverse along the axis with closely spaced circles
// double distLeft = corner.distance(corner.left);
// double distRight = corner.distance(corner.right);
// NodeInfo next = distLeft < distRight ? corner.left : corner.right;
NodeInfo next = corner.left;
next.marked = true;
// System.out.println("corner "+corner.ellipse.center);
// System.out.println("next "+next.ellipse.center);
List column0 = new ArrayList<>();
List column1 = new ArrayList<>();
bottomTwoColumns(corner, next, column0, column1);
if (column0.size() < 2 || column1.size() < 2) {
if (verbose) System.out.println("First two columns to small! " + column0.size() + " " + column1.size());
continue;
}
grid.add(column0);
grid.add(column1);
boolean error = false;
boolean even = true;
while (true) {
int expected = column0.size();
column0 = column1;
column1 = new ArrayList<>();
if (!addRemainingColumns(column1, column0, even))
break;
even = !even;
grid.add(column1);
if (expected != column1.size()) {
error = true;
if (verbose) System.out.println("Unexpected column length! " + expected + " " + column1.size());
break;
}
}
if (!error) {
if (grid.size() < 2)
continue;
if (checkDuplicates(grid)) {
if (verbose) System.out.println("contains duplicates");
continue;
}
saveResults(grid);
}
}
}
/**
* Pick a corner but avoid the pointy edges at the other end
*/
@Override NodeInfo selectSeedCorner() {
NodeInfo best = null;
double bestScore = 0;
double minAngle = Math.PI + 0.1;
for (int i = 0; i < contour.size; i++) {
NodeInfo info = contour.get(i);
if (info.angleBetween < minAngle)
continue;
Edge middleR = selectClosest(info.right, info, true);
if (middleR == null)
continue;
Edge middleL = selectClosest(info, info.left, true);
if (middleL == null)
continue;
if (middleL.target != middleR.target)
continue;
// With no perspective distortion, at the correct corners difference should be zero
// while the bad ones will be around 60 degrees
double r = UtilAngle.bound(middleR.angle + Math.PI);
double difference = UtilAngle.dist(r, middleL.angle);
double score = info.angleBetween - difference;
if (score > bestScore) {
best = info;
bestScore = score;
}
}
Objects.requireNonNull(best).marked = true;
return best;
}
private boolean addRemainingColumns( List column1, List column0, boolean even ) {
int start = 0;
if (even) {
NodeInfo second = selectClosestN(column0.get(0), column0.get(1));
if (second == null)
return false;
second.marked = true;
NodeInfo first = selectClosestN(column0.get(0), second);
if (first == null)
return false;
first.marked = true;
column1.add(first);
column1.add(second);
start = 1;
}
for (int i = start; i < column0.size() - 1; i++) {
NodeInfo n = selectClosestN(column0.get(i), column0.get(i + 1));
if (n != null) {
n.marked = true;
column1.add(n);
} else {
return false;
}
}
NodeInfo n = selectClosestN(column1.get(column1.size() - 1), column0.get(column0.size() - 1));
if (n == null)
return true;
n.marked = true;
column1.add(n);
return true;
}
/**
* Traverses along the first two columns and sets them up
*/
static void bottomTwoColumns( NodeInfo first, NodeInfo second, List column0, List column1 ) {
column0.add(first);
column0.add(second);
NodeInfo a = selectClosestN(first, second);
if (a == null) {
return;
}
a.marked = true;
column1.add(a);
NodeInfo b = second;
while (true) {
NodeInfo t = selectClosestN(a, b);
if (t == null) break;
t.marked = true;
column1.add(t);
a = t;
t = selectClosestN(a, b);
if (t == null) break;
t.marked = true;
column0.add(t);
b = t;
}
}
/**
* Finds the closest that is the same distance from the two nodes and part of an approximate equilateral triangle
*/
static @Nullable Edge selectClosest( NodeInfo a, NodeInfo b, boolean checkSide ) {
double bestScore = Double.MAX_VALUE;
Edge bestEdgeA = null;
Edge edgeAB = a.findEdge(b);
double distAB = a.distance(b);
if (edgeAB == null) {
return null;// TODO BUG! FIX!
}
for (int i = 0; i < a.edges.size; i++) {
Edge edgeA = a.edges.get(i);
NodeInfo aa = a.edges.get(i).target;
if (aa.marked) continue;
for (int j = 0; j < b.edges.size; j++) {
Edge edgeB = b.edges.get(j);
NodeInfo bb = b.edges.get(j).target;
if (bb.marked) continue;
if (aa == bb) {
// System.out.println("center "+aa.ellipse.center);
if (checkSide && UtilAngle.distanceCW(edgeAB.angle, edgeA.angle) > Math.PI*0.75)
continue;
double angle = UtilAngle.dist(edgeA.angle, edgeB.angle);
if (angle < 0.3)
continue;
double da = EllipsesIntoClusters.axisAdjustedDistanceSq(a.ellipse, aa.ellipse);
double db = EllipsesIntoClusters.axisAdjustedDistanceSq(b.ellipse, aa.ellipse);
da = Math.sqrt(da);
db = Math.sqrt(db);
// see if they are approximately the same distance
double diffRatio = Math.abs(da - db)/Math.max(da, db);
if (diffRatio > 0.3)
continue;
// TODO reject if too far
double d = (da + db)/distAB + 0.1*angle;
if (d < bestScore) {
bestScore = d;
bestEdgeA = a.edges.get(i);
}
break;
}
}
}
return bestEdgeA;
}
static @Nullable NodeInfo selectClosestN( NodeInfo a, NodeInfo b ) {
Edge e = selectClosest(a, b, true);
if (e == null)
return null;
else
return e.target;
}
/**
* Selects the closest node with the assumption that it's along the side of the grid.
*/
static @Nullable NodeInfo selectClosestSide( NodeInfo a, NodeInfo b ) {
double ratio = 1.7321;
NodeInfo best = null;
double bestDistance = Double.MAX_VALUE;
Edge bestEdgeA = null;
Edge bestEdgeB = null;
for (int i = 0; i < a.edges.size; i++) {
NodeInfo aa = a.edges.get(i).target;
if (aa.marked) continue;
for (int j = 0; j < b.edges.size; j++) {
NodeInfo bb = b.edges.get(j).target;
if (bb.marked) continue;
if (aa == bb) {
double da = EllipsesIntoClusters.axisAdjustedDistanceSq(a.ellipse, aa.ellipse);
double db = EllipsesIntoClusters.axisAdjustedDistanceSq(b.ellipse, aa.ellipse);
da = Math.sqrt(da);
db = Math.sqrt(db);
double max, min;
if (da > db) {
max = da;
min = db;
} else {
max = db;
min = da;
}
// see how much it deviates from the ideal length with no distortion
double diffRatio = Math.abs(max - min*ratio)/max;
if (diffRatio > 0.25)
continue;
// TODO reject if too far
double d = da + db;
if (d < bestDistance) {
bestDistance = d;
best = aa;
bestEdgeA = a.edges.get(i);
bestEdgeB = b.edges.get(j);
}
break;
}
}
}
// check the angles
if (best != null) {
Objects.requireNonNull(bestEdgeA); // sanity checks
Objects.requireNonNull(bestEdgeB);
double angleA = UtilAngle.distanceCW(bestEdgeA.angle, bestEdgeB.angle);
if (angleA < Math.PI*0.25) // expected with zero distortion is 30 degrees
return best;
else
return null;
}
return null;
}
/**
* Combines the inner and outer grid into one grid for output. See {@link Grid} for a discussion
* on how elements are ordered internally.
*/
void saveResults( List> graph ) {
Grid g = foundGrids.grow();
g.reset();
g.columns = graph.get(0).size() + graph.get(1).size();
g.rows = graph.size();
for (int row = 0; row < g.rows; row++) {
List list = graph.get(row);
for (int i = 0; i < g.columns; i++) {
if ((i%2) == (row%2))
g.ellipses.add(list.get(i/2).ellipse);
else
g.ellipses.add(null);
}
}
}
}