boofcv.alg.shapes.polyline.FitLinesToContour Maven / Gradle / Ivy
Go to download
Show more of this group Show more artifacts with this name
Show all versions of feature Show documentation
Show all versions of feature Show documentation
BoofCV is an open source Java library for real-time computer vision and robotics applications.
/*
* Copyright (c) 2011-2015, 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.shapes.polyline;
import boofcv.misc.CircularIndex;
import georegression.fitting.line.FitLine_F64;
import georegression.geometry.UtilLine2D_F64;
import georegression.geometry.UtilPoint2D_F64;
import georegression.metric.Intersection2D_F64;
import georegression.struct.line.LineGeneral2D_F64;
import georegression.struct.line.LinePolar2D_F64;
import georegression.struct.point.Point2D_F64;
import georegression.struct.point.Point2D_I32;
import org.ddogleg.struct.FastQueue;
import org.ddogleg.struct.GrowQueue_I32;
import java.util.Arrays;
import java.util.List;
/**
* Refines a set of corner points along a contour by fitting lines to the points between the corners using a
* least-squares technique. It then refines the corners estimates by interesting the lines and finding
* the closest point on the contour.
*
* A surprising number of things can go wrong and there are a lot of adhoc rules in this class and probably valid
* shapes are rejected. It's well tested but wouldn't be shocked if it contains bugs that are compensated for else
* where in the code.
*
* @author Peter Abeles
*/
public class FitLinesToContour {
// maximum number of samples along a line. After a certain point little is gained by sampling all of those
// points and it becomes very computationally expensive
int maxSamples = 20;
// number of iterations it will perform before giving up
int maxIterations = 5;
// minimum number of pixels a line must have for it to be fit
int minimumLineLength = 4;
// reference to the list of contour pixels
List contour;
// storage for working space
FastQueue lines = new FastQueue(LineGeneral2D_F64.class,true);
FastQueue pointsFit = new FastQueue(Point2D_F64.class,true);
private LinePolar2D_F64 linePolar = new LinePolar2D_F64();
private Point2D_F64 intersection = new Point2D_F64();
private GrowQueue_I32 workCorners = new GrowQueue_I32();
int anchor0;
int anchor1;
boolean verbose = false;
public void setContour(List contour) {
this.contour = contour;
}
/**
* Fits line segments along the contour with the first and last corner fixed at the original corners. The output
* will be a new set of corner indexes. Since the corner list is circular, it is assumed that anchor1 comes after
* anchor0. The same index can be specified for an anchor, it will just go around the entire circle
*
* @param anchor0 corner index of the first end point
* @param anchor1 corner index of the second end point.
* @param corners Initial location of the corners
* @param output Optimized location of the corners
*/
public boolean fitAnchored( int anchor0 , int anchor1 , GrowQueue_I32 corners , GrowQueue_I32 output )
{
this.anchor0 = anchor0;
this.anchor1 = anchor1;
int numLines = anchor0==anchor1? corners.size() : CircularIndex.distanceP(anchor0,anchor1,corners.size);
if( numLines < 2 ) {
throw new RuntimeException("The one line is anchored and can't be optimized");
}
lines.resize(numLines);
if( verbose ) System.out.println("ENTER FitLinesToContour");
// Check pre-condition
// checkDuplicateCorner(corners);
workCorners.setTo(corners);
for( int iteration = 0; iteration < maxIterations; iteration++ ) {
// fit the lines to the contour using only lines between each corner for each line
if( !fitLinesUsingCorners( numLines,workCorners) ) {
return false;
}
// intersect each line and find the closest point on the contour as the new corner
if( !linesIntoCorners(numLines, workCorners) ) {
return false;
}
// sanity check to see if corner order is still met
if( !sanityCheckCornerOrder(numLines, workCorners) ) {
return false; // TODO detect and handle this condition better
}
// TODO check for convergence
}
if( verbose ) System.out.println("EXIT FitLinesToContour. "+corners.size()+" "+workCorners.size());
output.setTo(workCorners);
return true;
}
// /**
// * Throws an exception of two corners in a row are duplicates
// */
// private void checkDuplicateCorner(GrowQueue_I32 corners) {
// for (int i = 0; i < corners.size();) {
// int j = (i+1)%corners.size();
// int index0 = corners.get(i);
// int index1 = corners.get(j);
//
// Point2D_I32 a = contour.get(index0);
// Point2D_I32 b = contour.get(index1);
//
// if( a.x == b.x && a.y == b.y ) {
// throw new RuntimeException("Duplicate corner!");
// } else {
// i++;
// }
// }
// }
/**
* All the corners should be in increasing order from the first anchor.
*/
boolean sanityCheckCornerOrder( int numLines, GrowQueue_I32 corners ) {
int contourAnchor0 = corners.get(anchor0);
int previous = 0;
for (int i = 1; i < numLines; i++) {
int contourIndex = corners.get(CircularIndex.addOffset(anchor0, i, corners.size()));
int pixelsFromAnchor0 = CircularIndex.distanceP(contourAnchor0, contourIndex, contour.size());
if (pixelsFromAnchor0 < previous) {
return false;
} else {
previous = pixelsFromAnchor0;
}
}
return true;
}
GrowQueue_I32 skippedCorners = new GrowQueue_I32();
/**
* finds the intersection of a line and update the corner index
*/
boolean linesIntoCorners( int numLines, GrowQueue_I32 contourCorners ) {
skippedCorners.reset();
// this is the index in the contour of the previous corner. When a new corner is found this is used
// to see if the newly fit lines point to the same corner. If that happens a corner is "skipped"
int contourIndexPrevious = contourCorners.get(anchor0);
for (int i = 1; i < numLines; i++) {
LineGeneral2D_F64 line0 = lines.get(i - 1);
LineGeneral2D_F64 line1 = lines.get(i);
int cornerIndex = CircularIndex.addOffset(anchor0, i, contourCorners.size);
boolean skipped = false;
// System.out.println(" corner index "+cornerIndex);
if (null == Intersection2D_F64.intersection(line0, line1, intersection)) {
if( verbose ) System.out.println(" SKIPPING no intersection");
// the two lines are parallel (or a bug earlier inserted NaN), so skip and remove one of them
skipped = true;
} else {
int contourIndex = closestPoint(intersection);
if( contourIndex != contourIndexPrevious ) {
Point2D_I32 a = contour.get(contourIndexPrevious);
Point2D_I32 b = contour.get(contourIndex);
if( a.x == b.x && a.y == b.y ) {
if( verbose ) System.out.println(" SKIPPING duplicate coordinate");
// System.out.println(" duplicate "+a+" "+b);
skipped = true;
} else {
// System.out.println("contourCorners[ "+cornerIndex+" ] = "+contourIndex);
contourCorners.set(cornerIndex, contourIndex);
contourIndexPrevious = contourIndex;
}
} else {
if( verbose ) System.out.println(" SKIPPING duplicate corner index");
skipped = true;
}
}
if( skipped ) {
skippedCorners.add( cornerIndex );
}
}
// check the last anchor to see if there's a duplicate
int cornerIndex = CircularIndex.addOffset(anchor0, numLines, contourCorners.size);
Point2D_I32 a = contour.get(contourIndexPrevious);
Point2D_I32 b = contour.get(contourCorners.get(cornerIndex));
if( a.x == b.x && a.y == b.y ) {
skippedCorners.add( cornerIndex );
}
// now handle all the skipped corners
Arrays.sort(skippedCorners.data,0,skippedCorners.size);
for (int i = skippedCorners.size-1; i >= 0; i--) {
int index = skippedCorners.get(i);
contourCorners.remove(index);
if( anchor0 >= index ) {
anchor0--;
}
if( anchor1 >= index ) {
anchor1--;
}
}
// cornerIndexes.size -= skippedCorners.size();
numLines -= skippedCorners.size;
for (int i = 0; i < numLines; i++) {
int c0 = CircularIndex.addOffset(anchor0, i, contourCorners.size);
int c1 = CircularIndex.addOffset(anchor0, i+1, contourCorners.size);
a = contour.get(contourCorners.get(c0));
b = contour.get(contourCorners.get(c1));
if( a.x == b.x && a.y == b.y ) {
throw new RuntimeException("Well I screwed up");
}
}
return contourCorners.size()>=3;
}
/**
* Fits lines across the sequence of corners
*
* @param numLines number of lines it will fit
*/
boolean fitLinesUsingCorners( int numLines , GrowQueue_I32 cornerIndexes) {
for (int i = 1; i <= numLines; i++) {
int index0 = cornerIndexes.get(CircularIndex.addOffset(anchor0, i - 1, cornerIndexes.size));
int index1 = cornerIndexes.get(CircularIndex.addOffset(anchor0, i, cornerIndexes.size));
if( index0 == index1 )
return false;
if (!fitLine(index0, index1, lines.get(i - 1))) {
// TODO do something more intelligent here. Just leave the corners as is?
return false;
}
LineGeneral2D_F64 l = lines.get(i-1);
if( Double.isNaN(l.A) || Double.isNaN(l.B) || Double.isNaN(l.C)) {
throw new RuntimeException("This should be impossible");
}
}
return true;
}
/**
* Given a sequence of points on the contour find the best fit line.
*
* @param contourIndex0 contour index of first point in the sequence
* @param contourIndex1 contour index of last point (exclusive) in the sequence
* @param line storage for the found line
* @return true if successful or false if it failed
*/
boolean fitLine( int contourIndex0 , int contourIndex1 , LineGeneral2D_F64 line ) {
int numPixels = CircularIndex.distanceP(contourIndex0,contourIndex1,contour.size());
// if its too small
if( numPixels < minimumLineLength )
return false;
Point2D_I32 c0 = contour.get(contourIndex0);
Point2D_I32 c1 = contour.get(contourIndex1);
double scale = c0.distance(c1);
double centerX = (c1.x+c0.x)/2.0;
double centerY = (c1.y+c0.y)/2.0;
int numSamples = Math.min(maxSamples,numPixels);
pointsFit.reset();
for (int i = 0; i < numSamples; i++) {
int index = i*(numPixels-1)/(numSamples-1);
Point2D_I32 c = contour.get( CircularIndex.addOffset(contourIndex0,index,contour.size()));
Point2D_F64 p = pointsFit.grow();
p.x = (c.x-centerX)/scale;
p.y = (c.y-centerY)/scale;
}
if( null == FitLine_F64.polar(pointsFit.toList(),linePolar) ) {
return false;
}
UtilLine2D_F64.convert(linePolar,line);
// go from local coordinates into global
line.C = scale*line.C - centerX*line.A - centerY*line.B;
return true;
}
/**
* Returns the closest point on the contour to the provided point in space
* @return index of closest point
*/
int closestPoint( Point2D_F64 target ) {
double bestDistance = Double.MAX_VALUE;
int bestIndex = -1;
for (int i = 0; i < contour.size(); i++) {
Point2D_I32 c = contour.get(i);
double d = UtilPoint2D_F64.distanceSq(target.x,target.y,c.x,c.y);
if( d < bestDistance ) {
bestDistance = d;
bestIndex = i;
}
}
return bestIndex;
}
}