Guide to implement our own algorithm for mapping/localization?

edit

If your goal is to implement your own mapping/localization algorithm, to my knowledge there is no real easy recipe, in the sense that you need some background in areas including:

• Motion modelling
• Sensor modelling
• Probability theory

That said, there is good material available on the web.

A good start would be to go check out the free Udacity course cs373 Artificial Intelligence for Robotics. It gives an implementation-driven approach to a range of localization/mapping/path planning/control algos. Good to get some hands-on intuition on complex concepts.

After that, you could for instance follow up with either (or both):

• the online course videos on localization and mapping from Cyrill Stachniss (available in a playlist on his Youtube channel).
• the book Probabilistic Robotics by Dieter Fox, Sebastian Thrun, and Wolfram Burgard (not free)

Both of those tackle localization, mapping and also control from the ground up, with in depth theory, generic algos and examples. They are more of a mouthful than the Udacity course, sure, but they also don't compare in the level of understanding they provide. If your goal is to implement (and understand!) custom localization and mapping based on concepts like EKF, UKF, particle filter, histograms or any Bayesian approach, it is a very good read.

On a side note, the online videos and book share a lot of material, so the main differences are the level of detail (+1 for the book) and the human-friendliness (+1 for the videos, if you have the patience to go through the hours of lesson).

I'm sure there is a lot more good material to start with, but these were a good start in my case.

Cheers