/** 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;
}
/**
* 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(20, 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;
}
/**
* 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;
}
/** finds the intersection of a line and update the corner index */
boolean linesIntoCorners(int numLines, GrowQueue_I32 contourCorners) {
GrowQueue_I32 skippedCorners = new GrowQueue_I32();
// System.out.println("total corners "+contourCorners.size()+" numLines "+numLines);
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();
for (int i = 0; i < contourCorners.size(); i++) {
int j = (i + 1) % contourCorners.size();
a = contour.get(contourCorners.get(i));
b = contour.get(contourCorners.get(j));
if (a.x == b.x && a.y == b.y) {
throw new RuntimeException("Well I screwed up");
}
}
return contourCorners.size() >= 3;
}