problems on double robots in graph

[DragonOnGit]

I am working on this program recent days, and I want to do some research on the graph slam. When I add a robot and also add a sonser on it, then I try to run slamtb_graph.m, it comes out a error which said

In solveGraphQR (line 60) In solveGraph (line 16) In slamtb_graph (line 236) Warning: The matrix is singularly working with precision. Wrong use atan2 The input must be a real number. Error q2au (line 21)          a = 2*atan2(n,s); Error q2v (line 17)      [a,u,A_q,U_q] = q2au(q); Error qpose2vpose (line 14) [v, V_q] = q2v(qp(4:7)); Error computeError (line 36)          [Fac.err.z, Z_pq] = qpose2vpose(pq); Error computeResidual (line 31)      [Fac(fac), e, W] =      computeError(Rob(rob),Sen(sen),Lmk(lmk),Obs(sen,lmk),Frm(frames),Fac(fac));      Error solveGraphQR (line 75)      [res, err_max] =      computeResidual(Rob,Sen,Lmk,Obs,Frm,Fac); Error solveGraph (line 16)          [Rob,Sen,Lmk,Obs,Frm,Fac] =          solveGraphQR(Rob,Sen,Lmk,Obs,Frm,Fac,Opt.solver);          Error slamtb_graph (line 236)          [Rob,Sen,Lmk,Obs,Frm,Fac] =          solveGraph(Rob,Sen,Lmk,Obs,Frm,Fac,Opt);

I want to know why this happened?Thank you!

[joansola]

Hello. Thanks for sharing this.

I wrote the graph section of the toolbox for a SLAM course, quite quickly. I did not provide the full capacity of features, and in particular, I never worked to make it multi-robot compatible. So I cannot help much here.

In multi-robot, you'll start having two disconnected problems, one for each robot. This means two separate graphs. The graphs will join once one robot sees a landmark that was initialized by another robot. You can see if you can walk your way through the code in order to make this possible.

Before this common landmark point, you have two separate graphs as said. This means that the Hessian matrix of the problem (in the Cholesky solving method -- the A matrix if you use QR) is singular. Matlab may be posing problems because of this.

Try then to walk you way through the solving stage and see if your matrices H or A are rank-deficient. They must be. Then, see how you can have Matlab not complain in these situations.

I would say these are the two things you need to take care of.

Please read the extensive PDF document on graph SLAM that you'll find in the toolbox website to have a better understanding of these mathematics. You can also have someone assist you on least squares methods, as the solving algorithm in the toolbox is the most basic one (Cholesky or QR based least squares).

If you happen to find a solution, I'll be very happy to hear from it and to accept any PR you want to create to enhance this toolbox.

Unfortunately I cannot dig into this code at this time.

Thank you

Joan

[DragonOnGit]

Thanks for your comment in such a hurry time. It's very helpful. Now I understand where I should dig into and work on. That's very helpful.

If I can do anything to find the solution, I would commit a PR.

Thank you again for your help. I will tyr my best.

Alex

[DragonOnGit]

Hello, professor, In term of my trying to fix the problem, I found that when I use Cholesky or Schur methods to solve the matrix problem, the function drawMapFig() made an error near the code below if FigOpt.map.showEllip && ~isempty(Map.pr) r = Rob(rob).state.r(1:3); P = Map.P(r,r); drawEllipse(MapFig.Rob(rob).ellipse,Rob(rob).frame.x(1:3),P); end where it said that 'pr' is not exist. I am trying to figure out that problem, and its very nice of you if you can give some information to me about this function if posssible. Thank you so much!

Best regard, Alex

[DragonOnGit]

Hello, professor, In term of my trying to fix the problem, I found that when I use Cholesky or Schur methods to solve the matrix problem, the function drawMapFig() made an error near the code below if FigOpt.map.showEllip && ~isempty(Map.pr) r = Rob(rob).state.r(1:3); P = Map.P(r,r); drawEllipse(MapFig.Rob(rob).ellipse,Rob(rob).frame.x(1:3),P); end where it said that 'pr' is not exist. I am trying to figure out that problem, and its very nice of you if you can give some information to me about this function if posssible. Thank you so much!

Best regard, Alex

Problem solved. Through adding LM method, it can solve with singular A matrix. But there are still many other bugs need to be fixed.

[joansola]

Drawing ellipses is only supported in QR, since the covariance needs to be computed, and this is only implemented in QR. So, use the QR solver, or disable drawing ellipses.

Did this solve your problem?