Assurance of ICP, internal Metrics

Question

So I have an iterative closest point (ICP) algorithm that has been written and will fit a model to a point cloud. As a quick tutorial for those not in the know ICP is a simple algorithm that fits points to a model ultimately providing a homogeneous transform matrix between the model and points.

Here is a quick picture tutorial.

Step 1. Find the closest point in the model set to your data set:

Step 2: Using a bunch of fun maths (sometimes based on gradiant descent or SVD) pull the clouds closer together and repeat untill a pose is formed:

![Figure 2][2]

Now that bit is simple and working, what i would like help with is: How do I tell if the pose that I have is a good one?

So currently I have two ideas, but they are kind of hacky:

1. How many points are in the ICP Algorithm. Ie, if I am fitting to almost no points, I assume that the pose will be bad:

   But what if the pose is actually good? It could be, even with few points. I dont want to reject good poses:

So what we see here is that low points can actually make a very good position if they are in the right place.

So the other metric investigated was the ratio of the supplied points to the used points. Here's an example

Now we exlude points that are too far away because they will be outliers, now this means we need a good starting position for the ICP to work, but i am ok with that. Now in the above example the assurance will say NO, this is a bad pose, and it would be right because the ratio of points vs points included is:

2/11 < SOME_THRESHOLD

So thats good, but it will fail in the case shown above where the triangle is upside down. It will say that the upside down triangle is good because all of the points are used by ICP.

You don't need to be an expert on ICP to answer this question, i am looking for good ideas. Using knowledge of the points how can we classify whether it is a good pose solution or not?

Using both of these solutions together in tandem is a good suggestion but its a pretty lame solution if you ask me, very dumb to just threshold it.

What are some good ideas for how to do this?

PS. If you want to add some code, please go for it. I am working in c++.

PPS. Someone help me with tagging this question I am not sure where it should fall.

Shayshaya

How is a pose defined? Line segments? – Polymerous

As a Homogeneous transform matrix. – Shayshaya

Out of curiosity, have you found a solution? Have you used something offered in answers, or came up with something of your own? – Convulsant

No, we have come up with some other things but they aren't really working out. I was hoping for a few more answers. – Shayshaya

Just out of interest, are you saying you want to ensure your ICP solution is the globally most optimum solution? Thus you are trying to avoid the local minima that generally occurs when performing ICP? – Collective

@Collective Yeah, that would be nice. – Shayshaya

Answer 1

One possible approach might be comparing poses by their shapes and their orientation.

Shapes comparison can be done with Hausdorff distance up to isometry, that is poses are of the same shape if

d(I(actual_pose), calculated_pose) < d_threshold

where d_threshold should be found from experiments. As isometric modifications of X I would consider rotations by different angles - seems to be sufficient in this case.

Is poses have the same shape, we should compare their orientation. To compare orientation we could use somewhat simplified Freksa model. For each pose we should calculate values

{x_y min, x_y max, x_z min, x_z max, y_z min, y_z max}

and then make sure that each difference between corresponding values for poses does not break another_threshold, derived from experiments as well.

Hopefully this makes some sense, or at least you can draw something useful for your purpose from this.

Convulsant

I hadn't really read this properly until now. For the second suggestion, do you need the actual pose to do the comparison. We don't have the actual pose, so that wont work. – Shayshaya

Answer 2

ICP attempts to minimize the distance between your point-cloud and a model, yes? Wouldn't it make the most sense to evaluate it based on what that distance actually is after execution?

I'm assuming it tries to minimize the sum of squared distances between each point you try to fit and the closest model point. So if you want a metric for quality, why not just normalize that sum, dividing by the number of points it's fitting. Yes, outliers will disrupt it somewhat but they're also going to disrupt your fit somewhat.

It seems like any calculation you can come up with that provides more insight than whatever ICP is minimizing would be more useful incorporated into the algorithm itself, so it can minimize that too. =)

Update

I think I didn't quite understand the algorithm. It seems that it iteratively selects a subset of points, transforms them to minimize error, and then repeats those two steps? In that case your ideal solution selects as many points as possible while keeping error as small as possible.

You said combining the two terms seemed like a weak solution, but it sounds to me like an exact description of what you want, and it captures the two major features of the algorithm (yes?). Evaluating using something like error + B * (selected / total) seems spiritually similar to how regularization is used to address the overfitting problem with gradient descent (and similar) ML algorithms. Selecting a good value for B would take some experimentation.

Polymerous

Unfortunately basing your assurance that the pose is correct based on the value you are trying to minimize isn't a very good idea. Any time that the pose falls into a local minima it will show a good pose. In fact it will always show a good pose if the ICP has worked correctly which isn't very good. Good answer though. – Shayshaya

That doesn't quite make sense. Of course the algorithm can reach local minima but the value it's trying to minimize still won't be as small as it would be if it found the global optimum. – Polymerous

In a normal case ICP yes but with the threshold removal of points you will find that it is often similar or smaller then the global minima. You also have to remember that the model points don't exactly match the data points as well. We have tried this method and found that it wasn't very good for our purpose. – Shayshaya

So the actual algorithm does a minimization step, and then a thresholding->removal step afterwards? – Polymerous

No it does a point matching to find distance to closest point on the model, then it will decide not to use that point if it is not close enough (ie. is an outlier) – Shayshaya

Answer 3

Looking at your examples, it seems that one of the things that determines whether the match is good or not, is the quality of the points. Could you use/calculate a weighting factor in calculating your metric?

For example, you could weight down points which are co-linear / co-planar, or spatially close, as they probably define the same feature. That would perhaps allow your upside-down triangle to be rejected (as the points are in a line, and that not a great indicator of the overall pose) but the corner-case would be ok, as they roughly define the hull.

Alternatively, maybe the weighting should be on how distributed the points are around the pose, again trying to ensure you have good coverage, rather than matching small indistinct features.

Engel

Good suggestion, do you have any papers or references where this is done. Also im concerned about computational intensity, do you think this will be very intensive? How do you go about checking the coverage of the shape when the model is more complex then a triangle, say a truck or a plane? – Shayshaya

Pretty much making this up as I go along, though there might be something in the Siggraph proceedings, it feels like a familiar problem. Anyway, I'm thinking that maybe constructing a rough oct-tree would be fairly cheap, and you could use the number of leaf-nodes containing matching points as an indication of coverage. – Engel