4
$\begingroup$

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.

Improve this question
$\endgroup$
  • $\begingroup$ In the getA11 function, for example, I see numbers that are powered by 2. If that is so huge it doesn't fit in float, 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
2
$\begingroup$

I have not checked your code, but I have faced the same problem and this is what I found (again, I don't know if it could be the same problem as you).

One key point for ICP is where your 'zero' is defined. The result of the translation of the ICP highly depends on this, but the rotation does not. See the image below:

enter image description here

As you can see, the rotation in both cases is the same, but the translation changes completely depending on that 'zero'.

I would suggest you to check if your point coulds have global coordinates or relative coordinates. Normally, ICP will work fine if you use relative coordinates of the scans to your robot, making the robot pose as the 'zero'.

Hope this helps.

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

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