0
$\begingroup$

I have gone through The GraphSLAM Algorithm with Applications to Large-Scale Mapping of Urban Structures. I implemented the code for GraphSLAM. The fundamental formula of GraphSLAM is

$$\mu = \Omega^{-1}\xi$$

When I inverse $\Omega$ it gives me an error that Matrix is singular.

In the above document there are some hints how to avoid this error. But I failed to understand that. On the eighth page of the above document - on page 410 - they discuss it in section 4. The GraphSLAM Algorithm portion. If anybody can understand it, please help me to understand.

From the text:

In particular, line 2 in GraphSLAM_linearize initializes the information elements. The “infinite” information entry in line 3 fixes the initial pose $x_0$ to (0 0 0)$^T$ . It is necessary, since otherwise the resulting matrix becomes singular, reflecting the fact that from relative information alone we cannot recover absolute estimates.

I added my code here for better understanding

import java.io.IOException;
import org.ujmp.core.SparseMatrix;
import org.ujmp.core.Matrix;

import java.io.File;
import java.util.Scanner;

public class Test1 {
public static void main(String args[]) throws IOException {
    Matrix omega = SparseMatrix.Factory.zeros(5, 5);
    Matrix Xi=SparseMatrix.Factory.zeros(5,1);
    int i = 0, i1 = 0, k1 = 0, k2 = 0,l=0,l1=0;
    double[] timex = new double[5];
    double[] forwardx = new double[5];
    double[] angularx = new double[5];
    double[]x1=new double[5];
    double[]y1=new double[5];
    double[]theta1=new double[5];
    double []landx=new double[2];
    double[]landy=new double[2];
    double[] timeya = new double[2];
    double[] codea = new double[2];
    double[] rangea = new double[2];
    double[] bearinga = new double[2];
    Scanner x = new Scanner(new File("/home/froboticscse/IdeaProjects/UJMPtest/src/main/java/a.txt"));
    Scanner y = new Scanner(new File("/home/froboticscse/IdeaProjects/UJMPtest/src/main/java/b.txt"));
    while (x.hasNext()) {

        double time = x.nextDouble();
        double forward = x.nextDouble();
        double angular = x.nextDouble();
        timex[i] = time;
        forwardx[i] = forward;
        angularx[i] = angular;
        x1[i]=((forwardx[i]*0.006+Math.cos(0+(angularx[i]*0.006)/2)));
        y1[i]=((forwardx[i]*0.006+Math.sin(0+(angularx[i]*0.006)/2)));
        theta1[i]=(angularx[i]*0.006);
        i++;
    }

    while (y.hasNext()) {
        double timey = y.nextDouble();
        double code = y.nextDouble();
        double range = y.nextDouble();
        double bearing = y.nextDouble();
        timeya[i1] = timey;
        codea[i1] = code;
        rangea[i1] = range;
        bearinga[i1] = bearing;

        i1++;

    }
    while (k1 < timex.length && k2 < timeya.length) {
        if (timex[k1] < timeya[k2]) {
            omega.setAsDouble(1, k1, k1);
            omega.setAsDouble(1, k1, k1 + 1);
            Xi.setAsDouble(x1[l],k1,0);
            k1++;
            l++;
        } else if (timex[k1] == timeya[k2]) {
            landx[l1]=x1[k1]+rangea[k2]*(Math.cos(bearinga[k2]+theta1[k1]));
            omega.setAsDouble(1, k1, k2);
            omega.setAsDouble(1, k2, k1);
            Xi.setAsDouble((landx[l1]+x1[l1]),k1,0);
            k2++;
            l1++;
        } else {
            System.out.println("Nothing to add");
        }
       // System.out.println(Xi);
    }
    System.out.println(omega);
   /* Matrix mu=omega.inv().mtimes(Xi);
    System.out.println(mu);*/
}

}

$\endgroup$
  • $\begingroup$ Could you provide either more details or source code for your implementation? Also, you should note that when a matrix is singular it is not invertible. So, you are likely getting this error when you try to compute $\Omega^{-1}$. $\endgroup$ – koverman47 Jul 18 '18 at 17:28
  • 1
    $\begingroup$ I add the code for better understanding $\endgroup$ – Encipher Jul 18 '18 at 18:01
  • $\begingroup$ @koverman47 robotics.stackexchange.com/questions/16094/… Check this link. As I don't get my answer I repeat my question. $\endgroup$ – Encipher Jul 25 '18 at 17:30
2
$\begingroup$

Due to singularity in matrix (i.e. the condition of matrix during which determinant of matrix becomes zero), matrix cannot be inverted.

So you can possibly try pseudo-inverse. It gives most approximated form of inverse matrix. For more information visit this link: https://en.wikipedia.org/wiki/Moore%E2%80%93Penrose_inverse

$\endgroup$
0
$\begingroup$

Just as manna kalsariya suggested, you can try to use the pseudo inverse. The README of the Universal Java Matrix Package (see "Quick Start") shows that the pseudo inverse is implemented and available for you to use. Instead of Matrix mu=omega.inv().mtimes(Xi);, try Matrix mu=omega.pinv().mtimes(xi);.

$\endgroup$
  • $\begingroup$ pinv() working. But the author suggest something in his algorithm which is not related to pseudo inverse. I don't understand what he mean in the above picture. Can any one explain what author suggest? Also the answer I get after pinv() don't think this is right answer. $\endgroup$ – Encipher Jul 18 '18 at 18:42
  • $\begingroup$ @EllenaMori the pseudo-inverse won't be an exact inverse (because one doesn't exist) so it may not appear as you expected. $\endgroup$ – koverman47 Jul 18 '18 at 20:27

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.