joansola / slamtb Goto Github PK

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!

when I Run the main script >>slamtb
some error occured , in createLandmarks.m --line 23 : Lmk(lmk).state.dsize = Opt.map.lmkDSize;
but in userData.m --line 185 : Opt.map has no lmkDSize,
so is there a minor mistake? thanks : )

change pitch by this

e(1) = atan2(-_R(2,0), sqrt(_R(0,0)*_R(0,0) + _R(1,0)*_R(1,0)));

indices are 012 not 123