I have lots of doubts about GraphSLAM, The GraphSLAM Algorithm with Applications to Large-Scale Mapping of Urban Structures. When I practically implement it I get matrix singularity error.

I taken the data set from UTIAS Multi-Robot Cooperative Localization and Mapping Dataset.

Now this dataset contain 75000 odometer data and 5000 sensor data. Correspondence are known correspondence. As per the algorithm initially I think the information matrix should be 75015x75015 matrix. But practically, this is impossible to implement. I am using universal java matrix package.

Then I think the robot may come to the same position after roaming certain amount of time. So I have to identify the location which is same as previous location.

I watched Lecture 7: Visual Navigation for Flying Robots where there is a description of Iterative Closest Point algorithm. This algorithm identity the same location.

But I have some doubts about the lecture. The prof said

Given: Two corresponding point sets (Clouds)

$$P = \{p_1,...,p_n\}\text{ and }Q = \{q_1,...,,q_n\}$$

Where does he get those points? Why there is there two data sets? Does $P$ represent X axis and $Q$ represent Y axis?

I have odometer and sensor raw data. From which one do I create this point cloud?

Do I really need to use this technique (ICP) for my implementation?


1 Answer 1


Iterative closest point is a method to fuse two point clouds together. If Robot A drove around town and gathered some data, then Robot B drove around town and gathered some data, ICP would (theoretically) be able to join the two clouds together.

In reality, if A and B don't have a common view (didn't drive down the same street/segment), then ICP isn't going to produce anything meaningful. Even if they did observe some common terrain, you still need a pretty good guess of how those segments should be joined. ICP is an iterative closest point algorithm and is prone to getting "stuck" in local minima.

That is, without a good seed point (initial guess), an ICP algorithm might try to put your shirt on as pants, because there are two long cylinders (sleeves) for your legs and one big opening for your waist. It fits better there than it does around your feet or upside down on your torso, so it "converges" on shirt == pants.

You've asked several questions now about a colossal data set - I would suggest trying with a smaller data set first and verifying whether the algorithm works or not, how you implement it, etc. You could try the first N samples, or average to every Nth sample, etc. Trying to go from no good guidance to completely solving what appears to be a large problem is a good recipe for getting frustrated and giving up. Take smaller steps.

  • $\begingroup$ Sir, this is very helpful explanation. So ICP is for two robots. I previously think it can be implemented over one Robot. If the robot visit the same place again and again it can detect this environment. So this is nothing related with it. So which algorithm is used to detect the loop closure in Slam problem? As per my knowledge loop closure means if the robot back to same environment this is a loop for this Robot. Am I right? $\endgroup$
    – Encipher
    Aug 6, 2018 at 17:21
  • $\begingroup$ @EllenaMori - Not necessarily two robots, but two point clouds. A point cloud is just indexed sensor data. Take the vehicle position and orientation (pose), build a 4x4 transform $T$ that describes the current pose, and use that to transform the raw sensor data back to a world coordinate. As the vehicle traverses space the sensor data gets indexed back to different coordinates and builds the 3D data set ("cloud"). There are lots of ways to do loop closure; ICP is just one of them. You would need to test current data against all previous data, though, as loop closure can happen anywhere. $\endgroup$
    – Chuck
    Aug 6, 2018 at 18:05

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

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