Better Cylinder Model Sampling

[akretz]

I have looked into the cylinder model sampling code and found the following two issues:

1. The cylinder axis direction is computed as follows: We take the line described by p1 & n1 and the line described by p2 & n2 and find the two points on each line where the lines are closest to each other. Then we take the vector from one of these points to the other. The problem is if these two points are very close or if the lines even intersect, we get a vector close to zero (or actually the zero vector). We can just calculate n1.cross (n2) and get the same direction without any issues if the lines intersect.
2. The cylinder radius is estimated using p1 only. That means we arbitrarily give one point a higher confidence than the other. On average, we get a better radius estimate by using both points for the estimate. If we do that, we also get the nice property that the model estimate is invariant to the order of the sample points (which is a good thing to have in RANSAC).

Since it is not obvious that the direction vector is just the cross product, I have written a SymPy script to verify the equivalence of both expressions.

from sympy import symbols, Matrix

n1 = Matrix(symbols("p1_0 p1_1 p1_2", real=True))
n2 = Matrix(symbols("p2_0 p2_1 p2_2", real=True))
p1 = Matrix(symbols("n1_0 n1_1 n1_2", real=True))
p2 = Matrix(symbols("n2_0 n2_1 n2_2", real=True))

w = n1 + p1 - p2
a = n1.dot(n1)
b = n1.dot(n2)
c = n2.dot(n2)
d = n1.dot(w)
e = n2.dot(w)
denominator = a * c - b * b

sc = (b * e - c * d) / denominator
tc = (a * e - b * d) / denominator

line_pt = p1 + n1 + sc * n1
line_dir = p2 + tc * n2 - line_pt

new_line_dir = n1.cross(n2)
new_denominator = new_line_dir.norm() ** 2

assert denominator.equals(new_denominator)
# Check if the direction vectors are equal up to scale
for i in range(3):
    assert (line_dir[i] / new_line_dir[i]).equals(
        line_dir[(i + 1) % 3] / new_line_dir[(i + 1) % 3]
    )

[mvieth]

Thanks!