package edu.stanford.rsl.tutorial.motion.estimation;
import ij.ImageJ;
import java.util.ArrayList;
import Jama.Matrix;
import edu.stanford.rsl.conrad.data.numeric.Grid2D;
import edu.stanford.rsl.conrad.geometry.shapes.simple.PointND;
import edu.stanford.rsl.conrad.numerics.SimpleMatrix;
import edu.stanford.rsl.conrad.numerics.SimpleOperators;
import edu.stanford.rsl.conrad.numerics.SimpleVector;
/**
* This class provides methods to interpolate scattered data of any dimension
* using a thin plate spline. The kernel is correct for 3-D. Other dimensions
* will require a different kernel, e.g. r*r*ln(r) for 2-D
*
* @author Marco Boegel
*
*/
public class ThinPlateSplineInterpolation {
/**
* Input control points
*/
private ArrayList<PointND> gridPoints;
/**
* Corresponding values to the gridPoints
*/
private ArrayList<PointND> values;
/**
* Dimension of the points
*/
private int dim;
/**
* TPS Interpolation coefficients
*/
private SimpleMatrix coefficients;
/**
* Polynomial Ax+b
*/
private SimpleMatrix A;
/**
* Polynomial Ax+b
*/
private SimpleVector b;
private boolean debug = false;
public float[] getAsFloatPoints() {
float[] pts = new float[gridPoints.size()*dim];
for(int i = 0; i < gridPoints.size(); i++) {
for(int k = 0 ; k < dim; k++) {
pts[i*dim +k] = (float) gridPoints.get(i).get(k);
}
}
return pts;
}
public float[] getAsFloatA() {
float[] Af = new float[A.getRows()*A.getCols()];
for(int i = 0; i < A.getCols(); i++) {
for(int j = 0; j < A.getRows(); j++) {
Af[i*A.getRows()+j] = (float) A.getElement(j, i);
}
}
return Af;
}
public float[] getAsFloatB() {
float [] Bf = new float[b.getLen()];
for(int i = 0; i < b.getLen(); i++) {
Bf[i] = (float) b.getElement(i);
}
return Bf;
}
public float[] getAsFloatCoeffs() {
float [] coeff = new float[coefficients.getCols()*coefficients.getRows()];
for(int i = 0; i < coefficients.getCols(); i++) {
for(int j = 0; j < coefficients.getRows(); j++) {
coeff[i*coefficients.getRows()+j] = (float) coefficients.getElement(j, i);
}
}
return coeff;
}
public ThinPlateSplineInterpolation(int dimension,
ArrayList<PointND> points, ArrayList<PointND> values) {
this.gridPoints = points;
this.values = values;
this.dim = dimension;
estimateCoefficients();
}
/**
* This method accepts a new pointgrid for interpolation, and the
* corresponding values. It automatically re-estimates the interpolation
* coefficients
*
* @param points
* Interpolation grid
* @param values
* corresponding values for the points
* @param dimension
* Dimension
*/
public void setNewPointsAndRecalibrate(ArrayList<PointND> points,
ArrayList<PointND> values, int dimension) {
this.gridPoints = points;
this.values = values;
this.dim = dimension;
estimateCoefficients();
}
/**
* Estimates the coefficients for the augmented TPS interpolation (including
* polynomial)
*/
private void estimateCoefficients() {
long start = 0;
if (debug)
start = System.nanoTime();
int n = gridPoints.size();
int sizeL = dim * n + dim * dim + dim;
A = new SimpleMatrix(dim, dim);
b = new SimpleVector(dim);
Matrix LJama = new Matrix(sizeL,sizeL);
Matrix rhsJama = new Matrix(sizeL, 1);
for (int i = 0; i < n; i++) {
for (int j = 0; j < values.get(i).getDimension(); j++) {
rhsJama.set(i*dim+j, 0, values.get(i).getAbstractVector().getElement(j));
}
for (int j = i+1; j < n; j++) {
// symmetric matrix -- compute only upper triangle and write to both locations
double val = kernel(gridPoints.get(i), gridPoints.get(j));
for (int k = 0; k < dim; k++) {
int currI = i * dim + k;
int currJ = j * dim + k;
LJama.set(currI, currJ, val);
LJama.set(currJ, currI, val);
}
}
}
int offset = n*dim;
for (int i = 0; i < n; i++) {
for (int j = 0; j < dim; j++) {
double val = gridPoints.get(i).get(j);
for (int k = 0; k < dim; k++) {
// Symmetric matrix!
LJama.set(dim * i + k, offset + dim * j + k, val);
LJama.set(offset + dim * j + k, dim * i + k, val);
}
}
for (int k = 0; k < dim; k++) {
// Symmetric matrix!
LJama.set(dim * i + k, offset + dim * dim + k, 1.0);
LJama.set(offset + dim * dim + k, dim * i + k, 1.0);
}
}
if (debug){
System.out.println("Time for reshaping : " + (System.nanoTime()-start)/1e6);
start = System.nanoTime();
}
Jama.LUDecomposition cd = new Jama.LUDecomposition(LJama);
Matrix parametersJama = cd.solve(rhsJama);
if(debug)
System.out.println("Time for inversion JAMA: " + (System.nanoTime()-start)/1e6);
coefficients = new SimpleMatrix(dim, n);
for (int i = 0; i < n; i++) {
for (int j = 0; j < dim; j++) {
coefficients.setElementValue(j, i, parametersJama.get(i * dim + j, 0));
}
}
for (int i = 0; i < dim; i++) {
for (int j = 0; j < dim; j++) {
A.setElementValue(j, i, parametersJama.get(dim * n + i * dim + j, 0));
}
b.setElementValue(i, parametersJama.get(dim * n + dim * dim + i, 0));
}
}
/**
* Interpolation kernel. Implements only euclidean distance
*
* @param p1
* Point 1
* @param p2
* Point 2
* @return euclidean distance between two points
*/
private double kernel(PointND p1, PointND p2) {
return p1.euclideanDistance(p2);
}
/**
* Lifts the kernel to the required dimension for interpolation
*
* @param p1
* @param p2
* @return
*/
private SimpleMatrix G(PointND p1, PointND p2) {
SimpleMatrix G = new SimpleMatrix(dim, dim);
G.identity();
double val = kernel(p1, p2);
G.multiplyBy(val);
return G;
}
/**
* Interpolation function
*
* @param pt
* point at which the value should be interpolated
* @return value
*/
public SimpleVector interpolate(PointND pt) {
SimpleVector res = new SimpleVector(pt.getDimension());
res.zeros();
for (int i = 0; i < coefficients.getCols(); i++) {
SimpleVector Gc = SimpleOperators.multiply(
G(pt, gridPoints.get(i)), coefficients.getCol(i));
res = SimpleOperators.add(res, Gc);
}
SimpleVector Ax = SimpleOperators.multiply(A, pt.getAbstractVector());
res = SimpleOperators.add(res, Ax, b);
return res;
}
public ArrayList<PointND> getGridPoints() {
return gridPoints;
}
public void setGridPoints(ArrayList<PointND> gridPoints) {
this.gridPoints = gridPoints;
}
public ArrayList<PointND> getValues() {
return values;
}
public void setValues(ArrayList<PointND> values) {
this.values = values;
}
public SimpleMatrix getCoefficients() {
return coefficients;
}
public void setCoefficients(SimpleMatrix coefficients) {
this.coefficients = coefficients;
}
public SimpleMatrix getA() {
return A;
}
public void setA(SimpleMatrix a) {
A = a;
}
public SimpleVector getB() {
return b;
}
public void setB(SimpleVector b) {
this.b = b;
}
public static void main(String args[]) {
new ImageJ();
ArrayList<PointND> points = new ArrayList<PointND>();
points.add(new PointND(30, 30));
points.add(new PointND(60, 60));
points.add(new PointND(90, 90));
points.add(new PointND(120, 140));
points.add(new PointND(150, 90));
points.add(new PointND(180, 60));
points.add(new PointND(210, 30));
ArrayList<PointND> values = new ArrayList<PointND>();
values.add(new PointND(100));
values.add(new PointND(200));
values.add(new PointND(300));
values.add(new PointND(400));
values.add(new PointND(300));
values.add(new PointND(200));
values.add(new PointND(100));
for (int i = 0; i < 100; i++) {
points.add(new PointND(i * 5, 400));
values.add(new PointND(0, 0));
}
// for(int i = 0; i < 100;i++) {
// points.add(new PointND(i*5,499));
// values.add(new PointND(0,0));
// }
ThinPlateSplineInterpolation tps = new ThinPlateSplineInterpolation(2,
points, values);
tps.interpolate(new PointND(30, 210));
Grid2D grid = new Grid2D(500, 500);
for (int i = 0; i < 500; i++) {
for (int j = 0; j < 500; j++) {
grid.setAtIndex(i, j, (float) tps
.interpolate(new PointND(i, j)).getElement(0));
}
}
grid.show();
}
}
/*
* Copyright (C) 2010-2014 Marco B�gel
* CONRAD is developed as an Open Source project under the GNU General Public License (GPL).
*/