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);*/
}

}

Improve this question
$\endgroup$
3
  • $\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
    Commented Jul 18, 2018 at 17:28
  • 1
    $\begingroup$ I add the code for better understanding $\endgroup$
    – Encipher
    Commented Jul 18, 2018 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
    Commented Jul 25, 2018 at 17:30

2 Answers 2

Reset to default
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

Improve this answer
$\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);.

Improve this answer
$\endgroup$
2
  • $\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
    Commented Jul 18, 2018 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
    Commented Jul 18, 2018 at 20:27

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

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