I'm currently developing a SLAM software on a robot, and I tried the Scan Matching algorithm to solve the odometry problem.
I read this article : Metric-Based Iterative Closest Point Scan Matching for Sensor Displacement Estimation
I found it really well explained, and I strictly followed the formulas given in the article to implement the algorithm.
You can see my implementation in python there : ScanMatching.py
The problem I have is that, during my tests, the right rotation was found, but the translation was totally false. The values of translation are extremely high.
Do you have guys any idea of what can be the problem in my code ?
Otherwise, should I post my question on StackOverflow or on the Mathematics Stack Exchange ?
The ICP part should be correct, as I tested it many times, but the Least Square Minimization doesn't seem to give good results.
The parts that might be problematic are the function getAXX() to getBX() (starting at line 91).
As you noticed, I used many decimal.Decimal values, cause sometimes the max float was not big enough to contain some values.
getA11
function, for example, I see numbers that are powered by 2. If that is so huge it doesn't fit infloat
, probably the original value was also very large (i.e.pix
,piy
etc). Large floating point values have large errors, but besides that, are those values expected to be actually that large? Is there perhaps a mistake with units, like you should be working in meters, but you are actually working with micrometers or something? $\endgroup$ – Shahbaz Oct 20 '14 at 14:23