/* * Copyright (C) 2013 Dr. John Lindsay <jlindsay@uoguelph.ca> * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see <http://www.gnu.org/licenses/>. */ package photogrammetry; /** * * @author johnlindsay */ public class Normalize2DHomogeneousPoints { public static double[][] T; public static double[][] normalize(double[][] points) { if (points.length != 3) { return null; } int i, n; int numPoints = points[0].length; double[][] ret = new double[3][numPoints]; // For the finite points ensure homogeneous coords have scale of 1 for (i = 0; i < numPoints; i++) { if (!isPointAtInfinity(points[0][i])) { ret[0][i] = points[0][i] / points[2][i]; ret[1][i] = points[1][i] / points[2][i]; ret[2][i] = 1; } } // Shift the origin to the centroid double meanX = 0; double meanY = 0; n = 0; for (i = 0; i < numPoints; i++) { if (!isPointAtInfinity(points[0][i])) { meanX += ret[0][i]; meanY += ret[1][i]; n++; } } meanX = meanX / n; meanY = meanY / n; for (i = 0; i < numPoints; i++) { if (!isPointAtInfinity(points[0][i])) { ret[0][i] -= meanX; ret[1][i] -= meanY; } } double meanDist = 0; for (i = 0; i < numPoints; i++) { if (!isPointAtInfinity(points[0][i])) { meanDist += Math.sqrt(ret[0][i] * ret[0][i] + ret[1][i] * ret[1][i]); } } meanDist = meanDist / n; double scale = Math.sqrt(2) / meanDist; for (i = 0; i < numPoints; i++) { if (!isPointAtInfinity(points[0][i])) { ret[0][i] *= scale; ret[1][i] *= scale; } } T = new double[][]{{scale, 0, -scale * meanX}, {0, scale, -scale * meanY}, {0, 0, 1}}; return ret; } public static boolean isPointAtInfinity(double value) { if (value == Float.POSITIVE_INFINITY) { return true; } if (value == Float.NEGATIVE_INFINITY) { return true; } if (value == Double.POSITIVE_INFINITY) { return true; } if (value == Double.NEGATIVE_INFINITY) { return true; } return false; } }