Develop Reference

How to interpret the distance of the Iterative Closest Point (ICP) algorithm

I'm trying to develop an application which uses the ICP algorithm to find similarities between shapes drawn in a specific area, but I don't understand how to interpret the distance which I get at the end of it. This is the algorithm I used, starting only with a list of points: where w is the rotation angle and T=(Tx, Ty) is the translation vector. They are described as well: where: Now, I tried to draw several shapes and what I was excepting is that the result would have been close to 0 if the shapes were very similar. Instead, with these shapes I got a final value of 3515.334: With this one I got 6615.08: And final, with this (they are completely different) I got 454.54: Did I misunderstood the algorithm?
This is the implementation:
private void icp (int z, ArrayList<Pair<Float,Float>> points) {
ArrayList<Pair<Float,Float>> copia = segments.get(z);
ArrayList<Pair<Float,Float>> copia2 = points;
double x_trattino = 0.0;
double y_trattino = 0.0;
double x_trattino_primo = 0.0;
double y_trattino_primo = 0.0;
for (int i=0; i<copia.size(); i++) {
x_trattino+=copia.get(i).first;
y_trattino+=copia.get(i).second;
}
x_trattino = x_trattino*((double)1/copia.size());
y_trattino = y_trattino*((double)1/copia.size());
for (int i=0; i<copia2.size(); i++) {
x_trattino_primo+=copia2.get(i).first;
y_trattino_primo+=copia2.get(i).second;
}
x_trattino_primo = x_trattino_primo*((double)1/copia2.size());
y_trattino_primo = y_trattino_primo*((double)1/copia2.size());
double Sxx = 0.0;
double Syy = 0.0;
double Sxy = 0.0;
double Syx = 0.0;
int min = 0;
if (copia.size()>copia2.size()) min = copia2.size(); else min = copia.size();
for (int i=0; i<min; i++) {
Sxx+=((copia.get(i).first-x_trattino)*(copia2.get(i).first-x_trattino_primo));
}
for (int i=0; i<min; i++) {
Syy+=((copia.get(i).second-y_trattino)*(copia2.get(i).second-y_trattino_primo));
}
for (int i=0; i<min; i++) {
Sxy+=((copia.get(i).first-x_trattino)*(copia2.get(i).second-y_trattino_primo));
}
for (int i=0; i<min; i++) {
Syx+=((copia.get(i).second-y_trattino)*(copia2.get(i).first-x_trattino_primo));
}
double rotation = Math.toDegrees(Math.atan((Sxy-Syx)/(Sxx+Syy)));
double Tx = x_trattino_primo - (x_trattino*Math.cos(rotation)-y_trattino*Math.sin(rotation));
double Ty = y_trattino_primo - (x_trattino*Math.sin(rotation)+y_trattino*Math.cos(rotation));
double eDist = 0.0;
for (int i=0; i<min; i++) {
eDist = (Math.pow(copia.get(i).first*Math.cos(rotation)-copia.get(i).second*Math.sin(rotation)+Tx-copia2.get(i).first, 2.0)+Math.pow(copia.get(i).first*Math.sin(rotation)+copia.get(i).second*Math.cos(rotation)+Ty-copia2.get(i).second, 2.0));
}
System.out.println("EDIST: "+eDist);
}

Mistakes you have made
Dont use Math.toDegrees, skip that call completely, the value in the variable rotation should be in radians not degrees.
You have written "eDist = ..." instead of "eDist += ...".
You have assumed that some "defining points" should match in the two different shapes. This is probably one of the biggest problems in the code. Lets say that you have two lines, the first is split in two parts while the other is not. Like this
.--------.---------.
.------------------.
What you will try to compare are the shapes
.--------.
.------------------.
These two shapes will not be similar and hence not give a small distance. This implies that you can have two shapes that look very similar but your algorithm will not detect this. To make a more complicated example of what you are trying to match is if you have several points lying like this in the two shapes
p1--p2--p3----p4----------p5---p6
q1------q2----------------q3---q4
What you are trying to match is p1 to q1, p2 to q2, p3 to q3 and p4 to q4. This is obviously not satisfactory.
One way forward
Choose one of the shapes as the source and the other as the target. For each source point, find the closest point in the target. Now you have a pair of points which are the matching points P and P' that you should use in the formulas. Note that several points in the source can be paired to the same point in the target. Repeat this process until there is not enough improvement in the error distance.
There are other ways of approaching ICP, and there are several problems that can occur. One problem to point out is that if you use the above approach on the line example I gave you, the error distance might not become small, since the middle point is not close to any point in the other shape.
So the above suggestion is not close the be exhaustive of what can be done, but at least I've given you a start.

In a NxN matrix, what algorithm finds the shortest path between two nodes?

Being new to algorithms and having searched all over the web, including some answers on stackoverflow, I still find myself asking how I find the distance between those nodes in a simple matrix.
First of all, the simple matrix:
public class MatrixRoutes {
int[][] position; // matrix
int size;
MatrixRoutes(int dimentions) {
posicion = new int[dimentions][dimentions];
size= dimensiones;
}
}
I set the size of the matrix with a simple
MatrixRoutes r = new MatrixRoutes(5);
Cool! I have my empty grid!
Populating it with the most simple of data, distances:
r.position[0][1] = 1;
r.position[1][1] = 0;
r.position[0][2] = 2;
r.position[2][2] = 0;
r.position[0][3] = 3;
r.position[3][3] = 0;
r.position[0][4] = 4;
r.position[4][4] = 0;
r.position[0][5] = 5;
r.position[5][5] = 0;
There's my test distance matrix, all ready to be tested.
Alright, got my nodes with distances. Now it's a matter of finding the shortest distance. I've been reading about different algorithms and their implementations with Java.
I've wanted to implement Dijkstra's algorithm, but it seems to only accept one starting number, used as a distance variable? That's not what I need when I need the distance between two variables.
Here's my attempt at implementing the algorithm:
private static int buscarRutaMasRapida(MatrixRoutes g, int nodeOrigin, int nodeDestiny)
{
int[] found = new int[g.position.length];
boolean[] visitedNode = new boolean[g.position.length];
int max = 999;
for (int i = 0; i < g.position.length; i++)
{
mejor[i] = max;
visitedNode [i] = false;
}
found[nodeOrigin+ nodeDestiny] = nodeOrigin + nodeDestiny;
for(int i = 0; i < g.position.length; i++)
{
int min = max;
int nodoNow = nodeOrigin;
for (int j = 0; j < g.position.length; j++)
{
if (!visitedNode [j] && found [j] < min)
{
nodoNow = j;
min = found [j];
}
}
visitedNode [nodoNow ] = true;
for (int j = 0; j < g.position.length; j++)
{
if (g.position[nodoNow ][j] < max && found[nodoNow ] + g.position[nodoNow ][j] < found [j])
{
found[j] = found [nodoNow ] + g.position[nodoNow ][j];
}
}
}
return found [g.position.length - 2];
}
All I'm asking is someone who would know of an algorithm which would find the shortest distance between two nodes in either a normal adjacency matrix or distance matrix.

Dijkstra's is the (my) preferred route. It takes in one node and finds the shortest path to all other nodes. Usually for distance between two nodes one would create a check inside Dijkstra's to return when the desired "end" node is reached.
In fact, Wikipedia has very nice psuedocode for you to use.
So for instance in the first step of Dijkstra's, you need to find the distance from the origin point S, you would look at all distances from S to other nodes and put this into your priority queue:
queue.add(position[S][x]); //Do this for all x adjacent to S

If you need to store the distance between each point then repeated use of Dijstraka's algorithm is inefficient if the adjacency graph is dense.
Instead you want the Floyd-Warshall algorithm.
From the wikipedia page, Dijstraka's will have a running time of O(V E log V ) while Floyd-Warshall will be O(V^3). In a connected graph E is between V (singly connected) and V^2 (each node connected to every other node), so the best will really depend on your data.

Having trouble multiplying two matrices

Sorry,this is a homework problem. I am not good with maths, so I checked out some videos to understand how two matrices are multiplied. I came up with a formula, but I do not know what I am doing wrong? This question has been answered before, but I did not understand. Thank you.
case 3:
System.out.println("THE PRODUCT OF TWO MATRICES ARE: ");
for(i =0; i< arrayList.length; i++){
for(j =0; j< arrayList1.length; j++){
for(k =0; k < arrayList1.length;k++){
multiplication = arrayList[i][k] * arrayList1[k][j] + multiplication;
}
System.out.print(arrayList[i][j]+" ");
}
System.out.println();
}
break;

First of all you should understand that the multiplication of two matrices should result in a matrice (which not appear to be the case with your multiplication variable).
I suppose you have to program the basic implementation. Let's take a look at the following matrices.
A has n rows, and m columns; said to be a matrice n x m.
Similary, B has m rows and p columns (m x p matrice). The multiplication of A x B will give you a matrice n x p.
Note that if you want to do the multiplication A x B, the matrice A must have the same number of columns that the number of rows of the matrice B.
Now each value in the matrice AB (ith row and jth column) is computed as follow:
That said, let's take a look at the Java implementation (which is a pure translation of the mathematical formula).
public static int[][] multiply(int[][] matrixA, int[][] matrixB) {
int[][] result = new int[matrixA.length][matrixB[0].length];
for (int i = 0; i < result.length; i++) {
for (int j = 0; j < result[0].length; j++) {
for (int k = 0; k < matrixB.length; k++) {
result[i][j] += matrixA[i][k] * matrixB[k][j];
}
}
}
return result;
}
The result matrice is initialized at the right dimensions. Then the first two nested loop (with indices i and j) will loop through all the elements of elements of the resulting matrice. Then you just need the third loop to compute the sum.
You'd still need to check that the matrices you give as parameters have the correct length.
The algorithm used is pretty naive (O(n3) complexity). If you don't understand it, there's a lot of resources in the web that explains how it works; but that would more a mathematical question than a programming one.
Hope it helps ! :)

Java Vectors: how to quickly "symmetrify" a large chunk of a huge sparse matrix

I have a huge sparse matrix (about 500K x 500K entries, with approximately 1% of the values being non-zero.
I'm using #mikera's Vectorz library.
t is a SparseRowMatrix composed of SparseIndexedVector rows.
For this chunk of the matrix, I am computing weights for (i,j) where j>i, putting them into an array of double, then creating the SparseIndexedVector for the row from that array. I was trying to cache the weights so that for the parts of the row where j<i, I could look up the previously computed value for (j,i) and put that value in for (i,j), but that took too much memory. So I am now trying to basically just compute and fill in the upper triangle for that chunk of the matrix, and then "symmetrify" it later. The chunk is from n1 x n1 to n2 x n2 (where n2 - n1 =~ 100K).
Conceptually, this is what I need to do:
for (int i = n1; i < n2; i++) {
for (int j = i + 1; j < n2; j++) {
double w = t.get(i, j);
if (w > 0) {
t.set(j, i, w);
}
}
}
But the "random access" get and set operations are quite slow. I assume unsafeGet would be faster.
Would it improve my performance to do the j-loop as my outer loop and convert the row back to a double array, then add elements and then create a new SparseIndexedVector from that array and replaceRow it back in? Something like:
for (j = n1 + 1; j < n2; j++) {
double[] jRowData = t.getRow(j).asDoubleArray();
for (i = 1; i < j-1; i++) {
double w = t.unsafeGet(i,j);
if (w > 0) {
jRowData[i] = w;
}
}
SparseIndexedVector jRowVector = SparseIndexedVector.createLength(n);
jRowVector.setElements(jRowData);
t.replaceRow(j, jRowVector);
}
Would something like that likely be more efficient? (I haven't tried it yet, as testing things on such large arrays takes a long time, so I'm trying to get an idea of what is "likely" to work well first. I've tried various incarnations on a smaller array (1K x 1K), but I've found that what is faster on a small array is not necessarily the same as what is faster on a large array.)
Is there another approach I should take instead?
Also, since memory is also a large concern for me, would it be helpful at the end of the outer loop to release the array memory explicitly? Can I do that by adding jRowData = null;? I assume that would save time on GC but I'm not all that clear on how memory management works in Java (7 if it matters).
Thanks in advance for any suggestions you can provide.

A matrix is symmetric if m[x,y] is equal to m[y,x] for all x and y, so if you know that the matrix must be symmetric, storing both m[x,y] and m[y,x] is redundant.
You can avoid storing both by rearranging the inputs if they meet a certain condition:
void set(int row, int col, double value) {
if(col < row) {
//call set again with transposed col/row
return set(col, row, value);
//we now know that we're in the top half of the matrix, proceed like normal
...
}
Do something similar for the get method:
double get(int row, int col) {
if(col < row) {
//call get again with transposed col/row
return get(col, row);
//we now know that we're in the top half of the matrix, proceed like normal
...
return value;
}
This technique will allow you to both avoid storing redundant values, and force the matrix to be symmetric without having to do any extra processing.

The la4j library guarantees O(log n) (where the n is a dimension) performance for both get/set operations on sparse matrices like CRSMatrix or CCSMatrix. You can perform a small experiment - just compile your code from conceptual box and run it with la4j:
for (int i = n1; i < n2; i++) {
for (int j = i + 1; j < n2; j++) {
double w = t.get(i, j);
if (w > 0) {
t.set(j, i, w);
}
}
}
You said you have to handle 500K x 500K matrices. For this size you can expect log_2(500 000) ~ 18 internal binary search iterations will be performed at each call to get/set operations.
Just think about 18 loop iterations on a moderl JVM and modern CPU.
Hope this helps. Have fun.

multiply RealMatrix with Complex matrix

I am pretty new to Java and I am facing an issue I believe it can be mastered pretty easily.
I am generating a project with is linked to the Apache - Commons Math library.
Within the project, I'm making use quite a lot of RealMatrix objects. I have a method working as follows
public static RealMatrix DistCalc(RealMatrix YCoord, RealMatrix ZCoord){
RealMatrix Distance = new Array2DRowRealMatrix(YCoord.getRowDimension(),ZCoord.getRowDimension());
for(int ii = 0; ii < YCoord.getRowDimension(); ii++){
for(int jj = 0; jj < ZCoord.getRowDimension(); jj++){
Distance.setEntry(ii,jj,Math.sqrt((YCoord.getEntry(ii, 0) - YCoord.getEntry(jj, 0))*(YCoord.getEntry(ii, 0) - YCoord.getEntry(jj, 0)) + (ZCoord.getEntry(jj, 0) - ZCoord.getEntry(ii, 0))*(ZCoord.getEntry(jj, 0) - ZCoord.getEntry(ii, 0))));
}
}
return Distance;
}
and another one generating a certain Complex matrix,
// Define the random phase for the u- component
public static Complex[][] RandPhi(int N, int nFFT){
Complex[][] nn_u = new Complex[N][nFFT];
for(int ii = 0; ii < N; ii++){
for(int jj = 0; jj < nFFT; jj++){
nn_u[ii][jj] = new Complex(Math.cos(new Random().nextDouble()*2*Math.PI),Math.sin(new Random().nextDouble()*2*Math.PI));
}
}
return nn_u;
}
Now, I'd like multiplying column-wise the RealMatrix Distance with the Complex matrix nn_u: in the end I should come up with a Complex[N][nFFT] matrix.
Would you mind to shed some light?

I recommend that you create your own ComplexMatrix interface based on the RealMatrix interface, and that you then create your own Array2DRowComplexMatrix class based on the Array2DRowRealMatrix class. To create the class, simply download the source code, change the class name, change double data[][] to Complex data[][], and then update all references to data.
Either create a ComplexMatrix constructor that accepts a RealMatrix, or else include a multiply method with a RealMatrix parameter.
Commons should have all of the methods you need, you might just need to tweak their parameter/return types a bit.