boofcv.alg.fiducial.calib.squares.SquaresIntoRegularClusters Maven / Gradle / Ivy
Go to download
Show more of this group Show more artifacts with this name
Show all versions of recognition Show documentation
Show all versions of recognition Show documentation
BoofCV is an open source Java library for real-time computer vision and robotics applications.
/*
* Copyright (c) 2011-2016, 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 boofcv.misc.CircularIndex;
import georegression.geometry.UtilLine2D_F64;
import georegression.metric.Distance2D_F64;
import georegression.metric.Intersection2D_F64;
import georegression.metric.UtilAngle;
import georegression.struct.line.LineGeneral2D_F64;
import georegression.struct.line.LineSegment2D_F64;
import georegression.struct.point.Point2D_F64;
import georegression.struct.point.Vector2D_F64;
import georegression.struct.shapes.Polygon2D_F64;
import org.ddogleg.nn.FactoryNearestNeighbor;
import org.ddogleg.nn.NearestNeighbor;
import org.ddogleg.nn.NnData;
import org.ddogleg.struct.FastQueue;
import org.ddogleg.struct.RecycleManager;
import java.util.List;
/**
* Processes the detected squares in the image and connects them into clusters. Squares can be connected to each
* other if two equivalent sides are parallel and their distance apart is "reasonable". The parallel requirement
* take advantage of line under perspective distortion remaining parallel.
*
* @author Peter Abeles
*/
public class SquaresIntoRegularClusters extends SquaresIntoClusters {
// maximum neighbors on nearest-neighbor search
public int maxNeighbors;
// tolerance for fractional distance away a point can be from a line to be considered on the line
double distanceTol = 0.2;
// maximum distance two squares can be from each other relative to the size of a square
double maxNeighborDistanceRatio;
// ratio of the length of a square to the distance separating the square
private double spaceToSquareRatio;
protected RecycleManager edges = new RecycleManager(SquareEdge.class);
// Storage for line segments used to calculate center
private LineGeneral2D_F64 line = new LineGeneral2D_F64();
private Point2D_F64 intersection = new Point2D_F64();
// used to search for neighbors that which are candidates for connecting
private NearestNeighbor search = FactoryNearestNeighbor.kdtree();
private FastQueue searchPoints;
private FastQueue> searchResults = new FastQueue(NnData.class,true);
/**
* Declares data structures and configures algorithm
* @param spaceToSquareRatio Ratio of space between squares to square lengths
* @param maxNeighbors The maximum number of neighbors it will look at when connecting a node
* @param maxNeighborDistanceRatio Maximum distance away a neighbor can be from a square to be connected. Relative
* to the size of the square. Try 1.35
*/
public SquaresIntoRegularClusters(double spaceToSquareRatio, int maxNeighbors, double maxNeighborDistanceRatio) {
this.spaceToSquareRatio = spaceToSquareRatio;
this.maxNeighbors = maxNeighbors;
// avoid a roll over later on in the code
if( this.maxNeighbors == Integer.MAX_VALUE ) {
this.maxNeighbors = Integer.MAX_VALUE-1;
}
this.maxNeighborDistanceRatio = maxNeighborDistanceRatio;
searchPoints = new FastQueue(double[].class,true) {
@Override
protected double[] createInstance() {
return new double[2];
}
};
search.init(2);
}
/**
* Processes the unordered set of squares and creates a graph out of them using prior knowledge and geometric
* constraints.
* @param squares Set of squares
* @return List of graphs. All data structures are recycled on the next call to process().
*/
public List> process( List squares ) {
recycleData();
// set up nodes
computeNodeInfo(squares);
// Connect nodes to each other
connectNodes();
// Find all valid graphs
findClusters();
return clusters.toList();
}
void computeNodeInfo( List squares ) {
for (int i = 0; i < squares.size(); i++) {
SquareNode n = nodes.grow();
n.reset();
n.corners = squares.get(i);
if( n.corners.size() != 4 )
throw new RuntimeException("Sqaures have four corners not "+n.corners.size());
// does not assume CW or CCW ordering just that it is ordered
lineA.a = n.corners.get(0);
lineA.b = n.corners.get(2);
lineB.a = n.corners.get(1);
lineB.b = n.corners.get(3);
// this will be the geometric center and invariant of perspective distortion
Intersection2D_F64.intersection(lineA, lineB, n.center);
for (int j = 0; j < 4; j++) {
int k = (j+1)%4;
double l = n.corners.get(j).distance(n.corners.get(k));
n.sideLengths[j] = l;
n.largestSide = Math.max(n.largestSide,l);
}
}
}
/**
* Goes through each node and uses a nearest-neighbor search to find the closest nodes in its local neighborhood.
* It then checks those to see if it should connect
*/
void connectNodes() {
setupSearch();
for (int i = 0; i < nodes.size(); i++) {
SquareNode n = nodes.get(i);
double[] point = searchPoints.get(i);
// distance between center when viewed head on will be space + 0.5*2*width.
// when you factor in foreshortening this search will not be symmetric
// the smaller will miss its larger neighbor but the larger one will find the smaller one.
double neighborDistance = n.largestSide*(1.0+spaceToSquareRatio)*maxNeighborDistanceRatio;
// find it's neighbors
searchResults.reset();
search.findNearest(point, neighborDistance*neighborDistance, maxNeighbors + 1, searchResults);
// try to attach it's closest neighbors
for (int j = 0; j < searchResults.size(); j++) {
NnData neighbor = searchResults.get(j);
if( neighbor.data != n )
considerConnect(n, neighbor.data);
}
}
}
/**
* Sets up data structures for nearest-neighbor search used in {@link #connectNodes()}
*/
private void setupSearch() {
searchPoints.reset();
for (int i = 0; i < nodes.size(); i++) {
SquareNode n = nodes.get(i);
double[] point = searchPoints.grow();
point[0] = n.center.x;
point[1] = n.center.y;
}
search.setPoints(searchPoints.toList(), nodes.toList());
}
/**
* Connects the 'candidate' node to node 'n' if they meet several criteria. See code for details.
*/
void considerConnect(SquareNode node0, SquareNode node1) {
// Find the side on each line which intersects the line connecting the two centers
lineA.a = node0.center;
lineA.b = node1.center;
int intersection0 = findSideIntersect(node0,lineA,lineB);
int intersection1 = findSideIntersect(node1,lineA,lineB);
if( intersection1 < 0 || intersection0 < 0 ) {
return;
}
// see if they have a similar shape
double sideSideRatio0 = node0.largestSide/node0.smallestSideLength();
double sideSideRatio1 = node1.largestSide/node1.smallestSideLength();
if( Math.abs(sideSideRatio0-sideSideRatio1) > 1.2 ) {
return;
}
// compare the size of the two closest sides. They should be similarish
double closeSide0 = node0.sideLengths[intersection0];
double closeSide1 = node1.sideLengths[intersection1];
double ratio = closeSide0>closeSide1 ? closeSide1/closeSide0 : closeSide0/closeSide1;
if( ratio < 0.5 ) {
return;
}
double distanceApart = lineA.getLength();
// Checks to see if the two sides selected above are closest to being parallel to each other.
// Perspective distortion will make the lines not parallel, but will still have a smaller
// acute angle than the adjacent sides
if( !mostParallel(node0, intersection0, node1, intersection1)) {
return;
}
// The following two tests see if the end points which define the two selected sides are close to
// the line created by the end points which define the opposing side.
// Another way of saying this, for the "top" corner on the side, is it close to the line defined
// by the side "top" sides on both squares.
// just look at the code its easier than understanding that description
if( !areMiddlePointsClose(node0.corners.get(add(intersection0, -1)), node0.corners.get(intersection0),
node1.corners.get(add(intersection1, 1)), node1.corners.get(add(intersection1, 2)))) {
return;
}
if( !areMiddlePointsClose(node0.corners.get(add(intersection0,2)),node0.corners.get(add(intersection0,1)),
node1.corners.get(intersection1),node1.corners.get(add(intersection1,-1)))) {
return;
}
checkConnect(node0,intersection0,node1,intersection1,distanceApart);
}
/**
* Finds the side which intersects the line on the shape. The line is assumed to pass through the shape
* so if there is no intersection it is considered a bug
*/
int findSideIntersect( SquareNode n , LineSegment2D_F64 line , LineSegment2D_F64 storage ) {
for (int i = 0; i < 4; i++) {
int j = (i+1)%4;
storage.a = n.corners.get(i);
storage.b = n.corners.get(j);
if( Intersection2D_F64.intersection(line,storage,intersection) != null ) {
return i;
}
}
// bug but I won't throw an exception to stop it from blowing up a bunch
return -1;
}
/**
* Returns true if the two sides are the two sides on each shape which are closest to being parallel
* to each other. Only the two sides which are adjacent are considered
*/
boolean mostParallel( SquareNode a , int sideA , SquareNode b , int sideB ) {
double selected = acuteAngle(a,sideA,b,sideB);
if( selected > acuteAngle(a,sideA,b,add(sideB,1)) || selected > acuteAngle(a,sideA,b,add(sideB,-1)) )
return false;
if( selected > acuteAngle(a,add(sideA,1),b,sideB) || selected > acuteAngle(a,add(sideA,-1),b,sideB) )
return false;
return true;
}
/**
* Returns an angle between 0 and PI/4 which describes the difference in slope
* between the two sides
*/
Vector2D_F64 vector0 = new Vector2D_F64();
Vector2D_F64 vector1 = new Vector2D_F64();
double acuteAngle( SquareNode a , int sideA , SquareNode b , int sideB ) {
Point2D_F64 a0 = a.corners.get(sideA);
Point2D_F64 a1 = a.corners.get(add(sideA, 1));
Point2D_F64 b0 = b.corners.get(sideB);
Point2D_F64 b1 = b.corners.get(add(sideB, 1));
vector0.set(a1.x - a0.x, a1.y - a0.y);
vector1.set(b1.x - b0.x, b1.y - b0.y);
double acute = vector0.acute(vector1);
return Math.min(UtilAngle.dist(Math.PI, acute), acute);
}
/**
* Returns true if point p1 and p2 are close to the line defined by points p0 and p3.
*/
boolean areMiddlePointsClose( Point2D_F64 p0 , Point2D_F64 p1 , Point2D_F64 p2 , Point2D_F64 p3 ) {
UtilLine2D_F64.convert(p0,p3,line);
// (computed expected length of a square) * (fractional tolerance)
double tol1 = p0.distance(p1)*distanceTol;
// see if inner points are close to the line
if(Distance2D_F64.distance(line, p1) > tol1 )
return false;
double tol2 = p2.distance(p3)*distanceTol;
if( Distance2D_F64.distance(lineB, p2) > tol2 )
return false;
//------------ Now see if the line defined by one side of a square is close to the closest point on the same
// side on the other square
UtilLine2D_F64.convert(p0,p1,line);
if(Distance2D_F64.distance(line, p2) > tol2 )
return false;
UtilLine2D_F64.convert(p3,p2,line);
if(Distance2D_F64.distance(line, p1) > tol1 )
return false;
return true;
}
/**
* Checks to see if the two nodes can be connected. If one of the nodes is already connected to
* another it then checks to see if the proposed connection is more desirable. If it is the old
* connection is removed and a new one created. Otherwise nothing happens.
*/
void checkConnect( SquareNode a , int indexA , SquareNode b , int indexB , double distance ) {
if( a.edges[indexA] != null && a.edges[indexA].distance > distance ) {
detachEdge(a.edges[indexA]);
}
if( b.edges[indexB] != null && b.edges[indexB].distance > distance ) {
detachEdge(b.edges[indexB]);
}
if( a.edges[indexA] == null && b.edges[indexB] == null) {
connect(a,indexA,b,indexB,distance);
}
}
/**
* Performs addition in the cyclical array
*/
private static int add( int index , int value ) {
return CircularIndex.addOffset(index, value, 4);
}
}
© 2015 - 2025 Weber Informatics LLC | Privacy Policy