Torus ransac model

[lasdasdas]

Implementing Torus model for sample consensus.

resolves #5706

This issue is work in progress. There are several things that I would like to comment with you in regards to this issue.

• The choice of Vector3f vs Vector4f
• What to do with the normals
• Euler angles approach vs normal vector -> I agree, I need to change this before PR.
• Leaving or not the ransac examples, documenting properly and so on
• The projection model does two things. First it searches the closest point on the circle of the torus ( the larger radius or the radius of the ring) and then it finds the intersect on the surface of the torus from that point to the point that we want to project. There are definitely some holes in this approach, specially zero division in some points, which are already marked to change before merge. I have not really checked whether this is the right way or not, but I'm pretty sure it has to be the right way.
• Check that weird torus shapes also work, mainly self intersecting ones.

Everything is a bit messy as you can tell, I removed some stuff and hardcoded some other things just to clear the compilation output for the time being. Also the commits don't look too bright either, I will squash everything on a clean branch before merging.

I will definitively keep working on this PR by myself, your feedback is more than welcome but I can keep going for a while. If you find something that is fundamentally unsound, please let me know.

In regards to sources of the code itself. I have started by working on a copy of the cylinder model and changed whatever was required for this specific implementation. I haven't based this code on any other libraries or implementations, I just put the torus equation on the residual function.

[mvieth]

Some general comments for now:
Please make sure you don't make any changes in unrelated files (not even whitespace changes, e.g. CMakeLists.txt, sac_model_cylinder.h, sac_model_ellipse3d.h).
Please add a simple test in test/sample_consensus/test_sample_consensus_quadric_models.cpp

[lasdasdas]

I need to excuse myself again for such a messy PR. On the last stage of the implementation I will close this branch and I will make a PR with a squashed commit with minimal (the right amount of) modifications. For the time being, please allow me to work on this messy one.

In actuality, there are just a couple of important files to review.

• sac_model_torus.h
• sac_model_torus.hpp

The two important functions are now fully implemented.

• OptimizationFunctor->operator()
• computeModelCoefficients is also ready.

Also I have switched the angles on the model params to the normal and the results are just as good.

Now, finding a "minimal" amount of points, and a technique to solve the torus equation has been challenging. The torus can intersect on inner and outer faces, 8 variables to solve for. Two degrees of orientation. Also the fact that its not lineal is not helping either, of course.

For now, i am using heuristics on computeModelCoefficients:

• The center is based around the centroid of the samples. (Which is not necessarily correct, of course)
• The normal of the torus orientation is the normal of the best-fit plane (also, an honest estimation too...)
• And finally the radii of the torus also are estimated given the maximum and minumum distance to the centroid.

I have increased the sample_size to 20 which is way too big, I know, im just validating.

I am requesting a review again, I would like to know your opinion in genal terms of the approaches followed. I will optimize and clean up later.

Thanks @mvieth

[lasdasdas]

I cleaned up all the files, there is still a lot to do, as seen on the code, the test case is still rubbish. Regardless. This should be the "right" amount of files changed.

[mvieth]

I need to excuse myself again for such a messy PR. On the last stage of the implementation I will close this branch and I will make a PR with a squashed commit with minimal (the right amount of) modifications. For the time being, please allow me to work on this messy one.

A messy commit history is really not a problem. We have the option to squash all commits into one while merging this PR via GitHub's interface. Alternatively, you can use interactive rebase and a force push on this branch. So no need for a new branch 🙂

In actuality, there are just a couple of important files to review.

* sac_model_torus.h

* sac_model_torus.hpp

The two important functions are now fully implemented.

* OptimizationFunctor->operator()

* computeModelCoefficients is also ready.

Also I have switched the angles on the model params to the normal and the results are just as good.

Nice 👍

Now, finding a "minimal" amount of points, and a technique to solve the torus equation has been challenging. The torus can intersect on inner and outer faces, 8 variables to solve for. Two degrees of orientation. Also the fact that its not lineal is not helping either, of course.

For now, i am using heuristics on computeModelCoefficients:

* The center is based around the centroid of the samples. (Which is not necessarily correct, of course)

* The normal of the torus orientation is the normal of the best-fit plane (also, an honest estimation too...)

* And finally  the radii of the torus also are estimated given the maximum and minumum distance to the centroid.

To be honest, I am not really convinced that this works well. It is easy to imagine use cases where for example the centroid of the sample does not coincide with the center of the torus. A point cloud from a sensor could for example only see one part of the torus. So I think we really need an analytical solution, like the other models. However, I am not sure yet what a feasible approach could be. Looking at https://en.wikipedia.org/wiki/Torus#Geometry , this would technically mean solving a quartic equation, and we would also have to account for center and orientation ...
Maybe implementing this as a model with normals is more doable? That might also make sense because otherwise the sample size might have to be too large to compute the eight coefficients?

[lasdasdas]

You are right, occluded measurements will be a problem. I will research a bit more the "direct" solutions can I find. Yeah, normals are still not off the books, although I personally think it would be nice implementing this without them.

If you have any other comment, please let me know and I will try to address it.

[mvieth]

So without using normals, we could do the following: use the formula from https://en.wikipedia.org/wiki/Torus#Geometry (the one after "algebraically eliminating the square root ..."), then adding arbitrary rotation with this rotation matrix (however the psi angle is zero meaning A_Z is the identity matrix because we don't need rotation around the z-axis (symmetry axis) of the torus), and adding arbitrary translation as (tx, ty, tz), we would arrive at:
pow(pow(cos(theta)*x+sin(phi)*sin(theta)*y+cos(phi)*sin(theta)*z+tx, 2)+pow(cos(phi)*y-sin(phi)*z+ty, 2)+pow(-sin(theta)*x+sin(phi)*cos(theta)*y+cos(phi)*cos(theta)*z+tz, 2)+pow(R, 2)-pow(r, 2), 2)-4*pow(R, 2)*(pow(cos(theta)*x+sin(phi)*sin(theta)*y+cos(phi)*sin(theta)*z+tx, 2)+pow(cos(phi)*y-sin(phi)*z+ty, 2))=0
for every point. I don't think it is possible to invert that to explicitly calculate r, R, phi, theta, tx, ty, tz, so gradient descent might be a solution. However, we would probably need 7 sample points (7 parameters to estimate) to calculate a model, which I honestly think is too much (risk is too high to have an outlier among these points). So I would say that using normals is the better option ...

[lasdasdas]

its been a while. I have reviewed the current state of the art. It seems the best you can get is 4 points with normals to get all the parameters. Check out Lukacs, G. & Marshall, David & Martin, R.. (2001). Geometric Least-Squares Fitting of Spheres, Cylinders, Cones and Tori. for context. There are other libraries that follow this approach, but this implementation has been done on a fresh slate. It is incomplete but pretty close to completion. Let me know what you think.

[lasdasdas]

This is a toy function translated into python that i am using to validate. The main issues are that this approach is based on a second degree equation that yields two possible solutions. The way that i am using to discriminate good solution from the bad one is to estimate the standard deviation of the final internal radius calculation. The good one gives a very low stddev, the other one not so much. I did not add noise. But the approach seems promising.

def torusfromnormals(p0, p1, p2,p3, n0, n1, n2, n3):



  def crossDot(v1, v2, v3):
    return np.cross(v1, v2).dot(v3)


  a01 = crossDot(n0, n1, n2);
  b01 = crossDot(n0, n1, n3);
  a0  = crossDot(p2-p1, n0, n2);
  a1  = crossDot(p0-p2, n1, n2);
  b0  = crossDot(p3-p1, n0, n3);
  b1  = crossDot(p0-p3, n1, n3);
  a   = crossDot(p0-p2, p1-p0, n2);
  b   = crossDot(p0-p3, p1-p0, n3);

  # Solve for t0 and t1
  k = -(a1 - b1 * a01 / b01) / (a0 - b0 * a01 / b01)
  p = -(a - b * a01 / b01) / (a0 - b0 * a01 / b01)

  # Solve for second degree equation coefficients
  _a = b01 * k
  _b = b01 * p + b0 * k + b1
  _c = b0 * p + b
  print(_a, _b, _c)

  # Check for imaginary solutions (b^2 - 4ac < 0)
  if _b**2 - 4 * _a * _c < 0:
    print("Torus fitting failed")
    return False

  s0 = (-_b + np.sqrt(_b**2 - 4 * _a * _c)) / (2 * _a)
  s1 = (-_b - np.sqrt(_b**2 - 4 * _a * _c)) / (2 * _a)

  for s in [s0, s1]:
      t1 = s
      t0 = k *t1+p

      d = ((p1 + n1*t1) - (p0+n0*t0))
      d = d / np.linalg.norm(d)
      print("direction", d)


      # Direction vector calculation

      A = np.array([
        [d.dot(n0), 1],
        [d.dot(n1), 1],
        [d.dot(n2), 1],
        [d.dot(n3), 1]])


      B =  np.array(
        [[-d.dot(p0)],
        [-d.dot(p1)],
        [-d.dot(p2)],
        [-d.dot(p3)]])



      print(A)

      # Set up linear least squares system for r and D
      sol = np.linalg.lstsq(A, B, rcond=None)[0]

      r_ext = - sol[0]
      D = sol[1]
      print("r_ext", r_ext)

      # Centroid calculation 
      Pany = (p1 + n1*t1) 
      lambda_ = (-d.dot(Pany) - D) / d.dot(d)

      centroid = Pany + lambda_ * d
      print("centroid", centroid)

      r_int = np.sqrt((np.linalg.norm(p0 - r_ext*n0 - centroid)**2 +
                       np.linalg.norm(p1 - r_ext*n1 - centroid)**2 +
                       np.linalg.norm(p2 - r_ext*n2 - centroid)**2 +
                       np.linalg.norm(p3 - r_ext*n3 - centroid)**2) / 4);

      r_int_stddev = np.sqrt(
                      (
                       (r_int - np.linalg.norm(p0 - r_ext*n0 - centroid))**2 +
                       (r_int - np.linalg.norm(p1 - r_ext*n1 - centroid))**2 +
                       (r_int - np.linalg.norm(p2 - r_ext*n2 - centroid))**2 +
                       (r_int - np.linalg.norm(p3 - r_ext*n3 - centroid))**2
                       ) / 4);


      print("r_int", r_int)
      print("r_int_stddev", r_int_stddev)  # low stddev is the actual solution.






def create_torus():

    rot = Rot.from_euler('zyx', [[45, 45, 45]], degrees=True)
    off = np.random.uniform(low=0.5, high=13.3, size=(3,))
    print("off" , off)
    
    n0 = [0,0,1]
    print("nor", rot.apply(n0))



    
    R = 1
    r = 0.3
    pts = []
    nor = []

    for i in range(2000):
        theta = random.uniform(0, 1) * np.pi * 2
        phi = random.uniform(0, 1) * np.pi * 2

        pt = [(R + r*np.cos(theta))*np.cos(phi),
               (R + r*np.cos(theta))*np.sin(phi),
               r* np.sin(theta)]
        n = [np.cos(theta)*np.cos(phi),
              np.cos(theta)*np.sin(phi),
              np.sin(theta)
              ]
        pt = rot.apply(pt)[0]
        ptoff = [sum(x) for x in zip(pt, off)]

        n = rot.apply(n)[0]

        pts.append(ptoff)
        nor.append(n)
    
    fig = plt.figure()
    ax = fig.add_subplot(projection='3d')


    ax.scatter([p[0] for p in pts], [p[1] for p in pts], [p[2] for p in pts])
    ax.quiver(
            
[p[0] for p in pts], [p[1] for p in pts], [p[2] for p in pts],
[p[0] for p in nor], [p[1] for p in nor], [p[2] for p in nor], length=0.1 
            


            )
    return pts,  nor

pts, nor = create_torus()
torusfromnormals(
       np.array( pts[0]),
       np.array( pts[1]),
       np.array( pts[2]),
       np.array( pts[3]),
       np.array( nor[0]),
       np.array( nor[1]),
       np.array( nor[2]),
       np.array( nor[3]))

[mvieth]

First round of review, I will do more testing soon and come back with more comments 😄

[lasdasdas]

done

[lasdasdas]

This last commit is a bit a of a WIP, levenbergmarquadt is a bit caotic and I was testing if it still works, and if it does, what stablitity it has. It has been messy, this pr should address most of the comments, specifically, the torusClosest function with something more sensible.

[mvieth]

I tested some more and all seems to work nicely, just a few more comments. Please remember to mark this as "ready for review" when your are happy with everything.

[lasdasdas]

Hi Markus, thanks for the review again. The last commit was WIP although you brought excellent points. This commit implements the last pending TODOS, cleans up a bit, removes unused stuff and so on. I have removed the WIP tag, and I will focus from now on - on reviewing and the merge if that's OK with you.

I am still upset that the LM optimization is so extremely prone to local minima, but the tests are passing with reasonable performance so I will call it for now. I am playing around with SymPy on the Python implementation that I wrote for this because I wanna find the jacobians, and feed them to the optimizer, maybe that makes a difference. If I am successful, maybe I'll do a PR in the future for it.

I will keep reviewing, If you get a chance to review, I'd appreciate it.

[lasdasdas]

The jacobian of the torus parameters on a given point (pog), the 8 parameters in respect to the only equation, the torus equation, a 7 to 1 mapping.

void compute_jacobian(R, r, px, py, pz, nx, ny, nz, pogx, pogy, pogz,
                   double* jac) {
  double x0 = std::pow(nx, 2);
  double x1 = std::pow(ny, 2);
  double x2 = 2 * x1;
  double x3 = x0 + x2;
  double x4 = 4 * nz;
  double x5 = 2 * std::pow(nz, 2) + x4 + 2;
  double x6 = x3 + x5;
  double x7 = 1.0 / x6;
  double x8 = pogx - px;
  double x9 = x7 * x8;
  double x10 = 2 * nx;
  double x11 = ny * x10;
  double x12 = 2 * ny;
  double x13 = nz * x12 + x12;
  double x14 = pogz - pz;
  double x15 = x14 * x7;
  double x16 = -x0 - x5;
  double x17 = pogy - py;
  double x18 = x17 * x7;
  double x19 = x11 * x9 + x13 * x15 + x16 * x18;
  double x20 = nz * x10 + x10;
  double x21 = x0 - x2 - x5;
  double x22 = x11 * x18 + x15 * x20 + x21 * x9;
  double x23 = std::sqrt(std::pow(x19, 2) + std::pow(x22, 2));
  double x24 = 2 * x20;
  double x25 = -x3;
  double x26 = x13 * x18 + x15 * x25 + x20 * x9;
  double x27 = x26 * x7;
  double x28 = x19 * x7;
  double x29 = x22 * x7;
  double x30 = 2 * (-R + x23) / x23;
  double x31 = 2 * x27;
  double x32 = 4 * nx;
  double x33 = x15 * x32;
  double x34 = 2 * nz + 2;
  double x35 = std::pow(x6, -2);
  double x36 = x32 * x35;
  double x37 = x36 * x8;
  double x38 = x17 * x36;
  double x39 = x14 * x36;
  double x40 = x18 * x32;
  double x41 = 8 * ny;
  double x42 = x35 * x41;
  double x43 = x0 * x42;
  double x44 = (1.0 / 2.0) * x19;
  double x45 = -4 * nx * x7 * x8 - 2 * x14 * x34 * x7;
  double x46 = (1.0 / 2.0) * x22;
  double x47 = x42 * x8;
  double x48 = x17 * x42;
  double x49 = x14 * x42;
  double x50 = 16 * nx * x1;
  double x51 = x35 * x8;
  double x52 = x17 * x35;
  double x53 = 4 * ny;
  double x54 = -x4 - 4;
  double x55 = x24 * x54;
  double x56 = 2 * x54;
  double x57 = x52 * x56;
  double x58 = x14 * x35;
  double x59 = x56 * x58;
  double x60 = ny * x54;

  jac[0] = 2 * R - 2 * x23;
  jac[1] = -2 * r;
  jac[2] = -x24 * x27 + x30 * (-x11 * x28 - x21 * x29);
  jac[3] = -x13 * x31 + x30 * (-x11 * x29 - x16 * x28);
  jac[4] = -x25 * x31 + x30 * (-x13 * x28 - x20 * x29);
  jac[5] =
      x26 * (-x13 * x38 - x20 * x37 - x25 * x39 - x33 + 2 * x34 * x7 * x8) +
      x30 *
          (x44 * (4 * ny * x7 * x8 - x13 * x39 - x16 * x38 - x40 - x43 * x8) +
           x46 * (4 * ny * x17 * x7 - x17 * x43 - x20 * x39 - x21 * x37 - x45));
  jac[6] = x26 * (-x13 * x48 - x15 * x41 + 2 * x17 * x34 * x7 - x20 * x47 -
                  x25 * x49) +
           x30 * (x44 * (-x13 * x49 - x16 * x48 - x45 - x50 * x51) +
                  x46 * (-x20 * x49 - x21 * x47 + x40 - x41 * x9 - x50 * x52));
  jac[7] =
      x26 * (x13 * x57 + x18 * x53 + x25 * x59 + x32 * x9 + x51 * x55) +
      x30 * (x44 * (x13 * x59 + x15 * x53 + x16 * x57 + x18 * x56 + x37 * x60) +
             x46 * (x21 * x51 * x56 + x33 + x38 * x60 + x55 * x58 + x56 * x9));
}

According to sympy anyways. I don't know if it belongs in here, it is obviously computer generated, alla symforce.

[mvieth]

With the code from sympy, you would replace Eigen's NumericalDiff, right? Feel free to go ahead if you are interested, but please in a separate pull request after this one is merged. And we would have to verify the sympy code by comparing it to a numerical diff in a few test cases, and perhaps also write a benchmark (analytical diff should always be faster than numerical diff, but I have no experience with sympy).

[mvieth]

Thank you so much for your work!

[lasdasdas]

You got it @larshg . Thx for the review.