2

I want to implement Sparse Extended information slam. There is four step to implement it. The algorithm is available in Probabilistic Robotics Book at page 310, Table 12.3.

enter image description here

In this algorithm line no:13 is not very clear to me. I have 15 landmarks. So $\mu_t$ will be a vector of (48*1) dimension where (3*1) for pose. Now $H_t^i$ is a matrix whose columns are dynamic as per the algorithm it is (3j-3) and 3j. J is the values of landmarks 1 to 15. Now how could I multiply a dynamic quantity with a static one. There must be a error that matrix dimension mismatch when implement in matlab.

Please help me to understand the algorithm better.

Encipher
  • 125
  • 1
  • 6

1 Answers1

1

You are right, that pseudocode is not correct. In particular, the definition of $H_t^i$ in line $11$ should be changed; all the way on the right-hand side, it should have $3N - 3j$ columns of $0$s, rather than $3j$ columns of $0$s.

With that change, every matrix $H_t^i$ will have the same number of columns:

$$6 + 3j - 3 + 3N - 3j = 3 + 3N,$$

which evaluates to a total of $48$ in your case (because you have $N = 15$ landmarks). That's precisely the correct dimensionality required for matrix multiplication with your $\mu_t$ vector.

The version of the book that you linked to appears to be a fairly old draft. This webpage contains errata for the third edition for the book, in which page 393 corresponds to what was page 310 in your version of the book. The errata for that third edition of the book can be downloaded at the following URL: http://probabilistic-robotics.informatik.uni-freiburg.de/corrections/pg393.pdf

There you'll find the fix that I described above, but also some other fixes (most of them are just notational, adding bars over the $\mu$ vectors, but it looks like a more serious issue was additionally fixed in line 13, where a minus was changed to a plus).

Dennis Soemers
  • 9,894
  • 2
  • 25
  • 66
  • 1
    Oh Thank You sir. It help me a lot. It's really save my code. Can you please elaborate what is the need of $\H_t^i$ matrix? In Table 12.5 of Probabilistic Robotics what is the significant of $\mu_t$. I think it content pose and landmarks coordinates.If I am right then at Table 12.3 (the picture I given with this question)line no. 5,6 also give the co ordinate of observed landmarks. Which one should be taken.? – Encipher Sep 20 '18 at 21:49
  • 1
    @EllenaMori Your understanding of the contents of $\mu$ is correct. In lines 5, 6 of Table 12.3, they are initialized to the initial observations in cases where a landmark was never observed before. In Table 12.5, those estimates are updated using subsequent observations. I'm not 100% sure on an intuitive explanation of the $H$ matrix. I have once implemented some variant of SLAM during my studies multiple years ago, but am by no means an expert on SLAM unfortunately :( – Dennis Soemers Sep 21 '18 at 08:15