boofcv.alg.fiducial.calib.squares.SquareCrossClustersIntoGrids 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.squares;
import org.ddogleg.struct.DogArray;
import org.jetbrains.annotations.Nullable;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import static boofcv.misc.CircularIndex.addOffset;
/**
* Takes as input a set of unordered cross connected clusters and converts them into ordered grids with known numbers
* of rows and columns. The output will be valid "chessboard" pattern. When rows and columns are discussed in the
* code below it refers to both white and black squares in the chessboard. A row that starts with a white square
* is referred to as white and one which starts with a black square as black.
*
* @author Peter Abeles
*/
public class SquareCrossClustersIntoGrids {
DogArray grids = new DogArray<>(SquareGrid::new);
// indicates if a fatal error was found in the grid
protected boolean invalid;
/**
* Converts all the found clusters into grids, if they are valid.
*
* @param clusters List of clusters
*/
public void process( List> clusters ) {
grids.reset();
for (int i = 0; i < clusters.size(); i++) {
if (checkPreconditions(clusters.get(i)))
processCluster(clusters.get(i));
}
}
/**
* Checks basic preconditions.
* 1) No node may be linked two more than once
*/
protected boolean checkPreconditions( List cluster ) {
for (int i = 0; i < cluster.size(); i++) {
SquareNode n = cluster.get(i);
for (int j = 0; j < n.square.size(); j++) {
SquareEdge e0 = n.edges[j];
if (e0 == null)
continue;
for (int k = j + 1; k < n.square.size(); k++) {
SquareEdge e1 = n.edges[k];
if (e1 == null)
continue;
if (e0.destination(n) == e1.destination(n)) {
return false;
}
}
}
}
return true;
}
/**
* Converts the cluster into a grid data structure. If its not a grid then
* nothing happens
*/
protected void processCluster( List cluster ) {
invalid = false;
// handle a special case
if (cluster.size() == 1) {
SquareNode n = cluster.get(0);
if (n.getNumberOfConnections() == 0) {
SquareGrid grid = grids.grow();
grid.reset();
grid.columns = grid.rows = 1;
grid.nodes.add(n);
return;
}
}
for (int i = 0; i < cluster.size(); i++) {
cluster.get(i).graph = SquareNode.RESET_GRAPH;
}
SquareNode seed = findSeedNode(cluster);
if (seed == null)
return;
// find the first row
List firstRow;
if (seed.getNumberOfConnections() == 1) {
firstRow = firstRow1(seed);
} else if (seed.getNumberOfConnections() == 2) {
firstRow = firstRow2(seed);
} else {
throw new RuntimeException("BUG");
}
if (invalid || firstRow == null)
return;
// Add the next rows to the list, one after another
List> listRows = new ArrayList<>();// TODO remove memory declaration here
listRows.add(firstRow);
while (true) {
List previous = listRows.get(listRows.size() - 1);
if (!addNextRow(previous.get(0), listRows)) {
break;
}
}
if (invalid || listRows.size() < 2)
return;
// re-organize into a grid data structure
SquareGrid grid = assembleGrid(listRows);
// check the grids connectivity
if (grid == null || !checkEdgeCount(grid)) {
grids.removeTail();
}
}
/**
* Converts the list of rows into a grid. Since it is a chessboard pattern some of the grid
* elements will be null.
*/
private @Nullable SquareGrid assembleGrid( List> listRows ) {
SquareGrid grid = grids.grow();
grid.reset();
List row0 = listRows.get(0);
List row1 = listRows.get(1);
int offset = row0.get(0).getNumberOfConnections() == 1 ? 0 : 1;
grid.columns = row0.size() + row1.size();
grid.rows = listRows.size();
// initialize grid to null
for (int i = 0; i < grid.columns*grid.rows; i++) {
grid.nodes.add(null);
}
// fill in the grid
for (int row = 0; row < listRows.size(); row++) {
List list = listRows.get(row);
int startCol = offset - row%2 == 0 ? 0 : 1;
// make sure there is the expected number of elements in the row
int adjustedLength = grid.columns - startCol;
if (adjustedLength - adjustedLength/2 != list.size()) {
return null;
}
int listIndex = 0;
for (int col = startCol; col < grid.columns; col += 2) {
grid.set(row, col, list.get(listIndex++));
}
}
return grid;
}
/**
* Looks at the edge count in each node and sees if it has the expected number
*/
private boolean checkEdgeCount( SquareGrid grid ) {
int left = 0, right = grid.columns - 1;
int top = 0, bottom = grid.rows - 1;
for (int row = 0; row < grid.rows; row++) {
boolean skip = grid.get(row, 0) == null;
for (int col = 0; col < grid.columns; col++) {
SquareNode n = grid.get(row, col);
if (skip) {
if (n != null)
return false;
} else {
boolean horizontalEdge = col == left || col == right;
boolean verticalEdge = row == top || row == bottom;
boolean outer = horizontalEdge || verticalEdge;
int connections = n.getNumberOfConnections();
if (outer) {
if (horizontalEdge && verticalEdge) {
if (connections != 1)
return false;
} else if (connections != 2)
return false;
} else {
if (connections != 4)
return false;
}
}
skip = !skip;
}
}
return true;
}
/**
* Adds the first row to the list of rows when the seed element has only one edge
*/
@Nullable List firstRow1( SquareNode seed ) {
for (int i = 0; i < seed.square.size(); i++) {
if (isOpenEdge(seed, i)) {
List list = new ArrayList<>();
seed.graph = 0;
// Doesn't know which direction it can traverse along. See figure that out
// by looking at the node its linked to
int corner = seed.edges[i].destinationSide(seed);
SquareNode dst = seed.edges[i].destination(seed);
int l = addOffset(corner, -1, dst.square.size());
int u = addOffset(corner, 1, dst.square.size());
if (dst.edges[u] != null) {
list.add(seed);
if (!addToRow(seed, i, -1, true, list)) return null;
} else if (dst.edges[l] != null) {
List tmp = new ArrayList<>();
if (!addToRow(seed, i, 1, true, tmp)) return null;
flipAdd(tmp, list);
list.add(seed);
} else {
// there is only a single node below it
list.add(seed);
}
return list;
}
}
throw new RuntimeException("BUG");
}
/**
* Adds the first row to the list of rows when the seed element has two edges
*/
@Nullable List firstRow2( SquareNode seed ) {
int indexLower = lowerEdgeIndex(seed);
int indexUpper = addOffset(indexLower, 1, seed.square.size());
List listDown = new ArrayList<>();
List list = new ArrayList<>();
if (!addToRow(seed, indexUpper, 1, true, listDown)) return null;
flipAdd(listDown, list);
list.add(seed);
seed.graph = 0;
if (!addToRow(seed, indexLower, -1, true, list)) return null;
return list;
}
/**
* Given a node, add all the squares in the row directly below it. They will be ordered from "left" to "right". The
* seed node can be anywhere in the row, e.g. middle, start, end.
*
* @return true if a row was added to grid and false if not
*/
boolean addNextRow( SquareNode seed, List> grid ) {
List row = new ArrayList<>();
List tmp = new ArrayList<>();
int numConnections = numberOfOpenEdges(seed);
if (numConnections == 0) {
return false;
} else if (numConnections == 1) {
for (int i = 0; i < seed.square.size(); i++) {
SquareEdge edge = seed.edges[i];
if (edge != null) {
// see if the edge is one of the open ones
SquareNode dst = edge.destination(seed);
if (dst.graph != SquareNode.RESET_GRAPH)
continue;
// determine which direction to traverse along
int corner = edge.destinationSide(seed);
int l = addOffset(corner, -1, dst.square.size());
int u = addOffset(corner, 1, dst.square.size());
// Nodes in the seed's row should all be marked, so any unmarked nodes
// are ones you don't want to traverse down
if (isClosedValidEdge(dst, l)) {
if (!addToRow(seed, i, 1, false, tmp)) return false;
flipAdd(tmp, row);
} else if (isClosedValidEdge(dst, u)) {
if (!addToRow(seed, i, -1, false, row)) return false;
} else {
dst.graph = 0;
row.add(dst);
}
break;
}
}
} else if (numConnections == 2) {
int indexLower = lowerEdgeIndex(seed);
int indexUpper = addOffset(indexLower, 1, seed.square.size());
if (!addToRow(seed, indexUpper, 1, false, tmp)) return false;
flipAdd(tmp, row);
if (!addToRow(seed, indexLower, -1, false, row)) return false;
} else {
return false;
}
grid.add(row);
return true;
}
private void flipAdd( List tmp, List row ) {
for (int i = tmp.size() - 1; i >= 0; i--) {
row.add(tmp.get(i));
}
}
/**
* Returns the open corner index which is first. Assuming that there are two adjacent corners.
*/
static int lowerEdgeIndex( SquareNode node ) {
for (int i = 0; i < node.square.size(); i++) {
if (isOpenEdge(node, i)) {
int next = addOffset(i, 1, node.square.size());
if (isOpenEdge(node, next)) {
return i;
}
if (i == 0) {
int previous = node.square.size() - 1;
if (isOpenEdge(node, previous)) {
return previous;
}
}
return i;
}
}
throw new RuntimeException("BUG!");
}
static int numberOfOpenEdges( SquareNode node ) {
int total = 0;
for (int i = 0; i < node.square.size(); i++) {
if (isOpenEdge(node, i))
total++;
}
return total;
}
/**
* Is the edge open and can be traversed to? Can't be null and can't have
* the marker set to a none RESET_GRAPH value.
*/
static boolean isOpenEdge( SquareNode node, int index ) {
if (node.edges[index] == null)
return false;
int marker = node.edges[index].destination(node).graph;
return marker == SquareNode.RESET_GRAPH;
}
/**
* Does the edge point some place, but is closed
*/
static boolean isClosedValidEdge( SquareNode node, int index ) {
if (node.edges[index] == null)
return false;
int marker = node.edges[index].destination(node).graph;
return marker != SquareNode.RESET_GRAPH;
}
/**
* Given a node and the corner to the next node down the line, add to the list every other node until
* it hits the end of the row.
*
* @param n Initial node
* @param corner Which corner points to the next node
* @param sign Determines the direction it will traverse. -1 or 1
* @param skip true = start adding nodes at second, false = start first.
* @param row List that the nodes are placed into
*/
boolean addToRow( SquareNode n, int corner, int sign, boolean skip,
List row ) {
SquareEdge e;
while ((e = n.edges[corner]) != null) {
if (e.a == n) {
n = e.b;
corner = e.sideB;
} else {
n = e.a;
corner = e.sideA;
}
if (!skip) {
if (n.graph != SquareNode.RESET_GRAPH) {
// This should never happen in a valid grid. It can happen if two nodes link to each other multiple
// times. Other situations as well
invalid = true;
return false;
}
n.graph = 0;
row.add(n);
}
skip = !skip;
sign *= -1;
corner = addOffset(corner, sign, n.square.size());
}
return true;
}
/**
* Finds a seed with 1 or 2 edges.
*/
static SquareNode findSeedNode( List cluster ) {
SquareNode seed = null;
for (int i = 0; i < cluster.size(); i++) {
SquareNode n = cluster.get(i);
int numConnections = n.getNumberOfConnections();
if (numConnections == 0 || numConnections > 2)
continue;
seed = n;
break;
}
return Objects.requireNonNull(seed);
}
public DogArray getGrids() {
return grids;
}
}