Optimize distance computations for cylinder

[akretz]

I have optimized the running time of the cylinder sampling consensus algorithm. The bottleneck is the distance computation which finds inliers.

For the computation of the vector dir from the cylinder axis to the point, we can reduce the number of operations. We compute line_dir.cross3 (pt - line_pt) inside pointToLineDistance () and then throw it away. But keeping that cross product and then computing the cross product of that vector and line_dir gives us the vector we are looking for.

I have benchmarked these modifications on the example and had it run for 100,000 RANSAC iterations on a single core while removing the early return. The benchmarks on two different platforms with gcc and clang yield speedups of up to 2.6 (all built with -DCMAKE_BUILD_TYPE=MinSizeRel).

Platform	CPU	Compiler	Runtime master [s]	Runtime this PR [s]	Speedup
Ubuntu 24.04	Epyc 7002	gcc 13.3.0	178.9	154.9	1.2
Ubuntu 24.04	Epyc 7002	clang 18.1.3	151.3	58.9	2.6
macOS 15	M1 Max	gcc 15.1.0	68.1	46.1	1.5
macOS 15	M1 Max	clang 16.0.0	40.8	26.8	1.5

[larshg]

Hi @akretz

Looks good - I have ventured a bit further and opened a PR against yours on your fork. See akretz#1.

@mvieth it seems that the normals provided for the test is not correctly normalized - see

pcl/test/sample_consensus/test_sample_consensus_quadric_models.cpp

Line 431 in 96e9621

normals[0].getNormalVector3fMap() << 0.000098f, 1.000098f, 0.000008f;

Should we update them to be correctly normalized or should we let them stay as is - there could be removed some further normalizing in the various methods, but it would reduce robustness of the algorithm, with respect to handling ill-formed normals?

[mvieth]

I had a look at your proposed logic, and I think there might be an even more straightforward approach: I think it would be fastest to just compute the vector from the cylinder axis to the point via this formula: https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Vector_formulation If I did not make a mistake, it would be Eigen::Vector4f dir = (line_pt-pt)-((line_pt-pt).dot(line_dir)*line_dir. Then dir.norm() would replace pointToLineDistance (pt, model_coefficients) (or dir_cross_pt.norm () in your approach). Computing dir via this approach would use 6 multiplications and 6 subtractions, while your approach with the two cross-products would use 12 multiplications and 6 subtractions. Might be worth a try?

Also a heads-up: the code with the normals and the angle might not be covered by a unit test because normal_distance_weight_ is zero by default, and there is no call to setNormalDistanceWeight in test_sample_consensus_quadric_models.cpp

[akretz]

I had a look at your proposed logic, and I think there might be an even more straightforward approach: I think it would be fastest to just compute the vector from the cylinder axis to the point via this formula: https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Vector_formulation If I did not make a mistake, it would be Eigen::Vector4f dir = (line_pt-pt)-((line_pt-pt).dot(line_dir)*line_dir. Then dir.norm() would replace pointToLineDistance (pt, model_coefficients) (or dir_cross_pt.norm () in your approach). Computing dir via this approach would use 6 multiplications and 6 subtractions, while your approach with the two cross-products would use 12 multiplications and 6 subtractions. Might be worth a try?

Thanks for that! Your suggestion is a much more straightforward way to do that computation. I have modified my PR accordingly.

Also a heads-up: the code with the normals and the angle might not be covered by a unit test because normal_distance_weight_ is zero by default, and there is no call to setNormalDistanceWeight in test_sample_consensus_quadric_models.cpp

Good point! I have added checks for these functions in the test to verify that nothing breaks. I've had to change a test normal, because the angles between dir and normal are close to zero.

I have also cherry-picked @larshg's useful benchmark.

[larshg]

I had a look at your proposed logic, and I think there might be an even more straightforward approach: I think it would be fastest to just compute the vector from the cylinder axis to the point via this formula: https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Vector_formulation If I did not make a mistake, it would be Eigen::Vector4f dir = (line_pt-pt)-((line_pt-pt).dot(line_dir)*line_dir. Then dir.norm() would replace pointToLineDistance (pt, model_coefficients) (or dir_cross_pt.norm () in your approach). Computing dir via this approach would use 6 multiplications and 6 subtractions, while your approach with the two cross-products would use 12 multiplications and 6 subtractions. Might be worth a try?

This was also one of the optimizations that ChatGPT found 😄

Maybe it could be updated in:

pcl/common/include/pcl/common/distances.h

Line 75 in 96e9621

sqrPointToLineDistance (const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir)

Even though the current implementation works if line_dir is not normalized, which is the case in the test:

pcl/test/common/test_common.cpp

Line 141 in 96e9621

Eigen::Vector4f pt (1,0,0,0), line_pt (0,0,0,0), line_dir (1,1,0,0);

The sqrPointToLineDistance is used in the cylinder and cone in doSamplesVerifyModel, through an intermediate local method, which seems superfluous?

[larshg]

So a question again arise, should one assume that a direction is normalized? Its seems so in the cylinder model coefficients as it gets normalized, but should or should we not add that assumption in general - and update the documentation accordingly?

[mvieth]

Maybe it could be updated in:

pcl/common/include/pcl/common/distances.h

Line 75 in 96e9621

sqrPointToLineDistance (const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir)

I think there it would not bring any benefit: one cross product needs 6 multiplications and 3 subtractions (plus 3 subtractions for line_pt - pt), while the new approach with the dot-product needs 6 multiplications and 6 subtractions. The new approach is only useful in countWithinDistance etc. because we need the vector from the cylinder axis to the point for the normal-angle-calculation (in sqrPointToLineDistance we don't need that vector).

Even though the current implementation works if line_dir is not normalized, which is the case in the test:

pcl/test/common/test_common.cpp

Line 141 in 96e9621

Eigen::Vector4f pt (1,0,0,0), line_pt (0,0,0,0), line_dir (1,1,0,0);

The sqrPointToLineDistance is used in the cylinder and cone in doSamplesVerifyModel, through an intermediate local method, which seems superfluous?

So a question again arise, should one assume that a direction is normalized? Its seems so in the cylinder model coefficients as it gets normalized, but should or should we not add that assumption in general - and update the documentation accordingly?

Not sure if I understand you correctly - the direction of the cylinder axis gets normalized (e.g. in countWithinDistance), so there is no assumption that the user passed in model coefficients with the direction already normalized. I would keep it that way since it is more robust, and normalizing the axis direction once costs very little compared to the rest of the function. However I also do not really see the need to explicitly say in the documentation that the direction does not have to be normalized? I works either way.

[mvieth]

Two minor suggestions, otherwise looks good to me. Thank you!

[larshg]

Maybe it could be updated in:

pcl/common/include/pcl/common/distances.h

Line 75 in 96e9621

sqrPointToLineDistance (const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir)

I think there it would not bring any benefit: one cross product needs 6 multiplications and 3 subtractions (plus 3 subtractions for line_pt - pt), while the new approach with the dot-product needs 6 multiplications and 6 subtractions. The new approach is only useful in countWithinDistance etc. because we need the vector from the cylinder axis to the point for the normal-angle-calculation (in sqrPointToLineDistance we don't need that vector).

  double inline
  sqrPointToLineDistance (const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir)
  {
    // Calculate the distance from the point to the line
    // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p1-p0)) / norm(p2-p1)
    const Eigen::Vector3f diff = (line_pt - pt).head<3>();           // Used in both methods (3 sub)

    const auto crossProduct = line_dir.cross3(diff);                               // 6 mul + 3 sub
    const auto crossProductNormSquared = crossProduct.squaredNorm();               // 3 mul + 2 add
    const auto lineDirNormSquared = line_dir.squaredNorm();                        // 3 mul + 2 add
    const auto crossDistanceResult = crossProductNormSquared / lineDirNormSquared; // 1 div

    // operation sum: 12 mul + 4 add + 3 sub + 1 div

    // Optimized form: ||diff||^2 - (diff·dir)^2 / ||dir||^2
    const Eigen::Vector3f dir = line_dir.head<3>();
    const float dir_len_sq = dir.squaredNorm();             // 3 mul + 2 add
    if (dir_len_sq <= std::numeric_limits<float>::epsilon())
      return 0.0; // Degenerate direction; previous implementation would have produced
                  // inf/NaN
    const float proj = diff.dot(dir);                       // 3 mul + 2 add
    const auto diffSquaredNorm = diff.squaredNorm();        // 3 mul + 2 add
    return static_cast<double>(diffSquaredNorm -
                               (proj * proj) / dir_len_sq); // 1 mul + 1 div + 1 sub
    // operation sum: 10 mul + 6 add + 1 sub + 1 div
  }

I have splitted the operations and I get this count - so its less operations? And I think ie. a dot product can fit into a FMA operations, which could result in faster code?

But it requires the line_dir to be normalized which would increase the number of operations, unless it is done outside a loop etc.

It would also be possible to calculate 1 / dir_len_sq before entering a loop (for many points), as long as the line_dir doesn't change. Then one could remove the div operation and save a squaredNorm calculation, ie 3 mul and 2 adds also?

But that would require some more changes to the api - or not use this api, but do the calculations in cylinder file.

Even though the current implementation works if line_dir is not normalized, which is the case in the test:

pcl/test/common/test_common.cpp

Line 141 in 96e9621

Eigen::Vector4f pt (1,0,0,0), line_pt (0,0,0,0), line_dir (1,1,0,0);

The sqrPointToLineDistance is used in the cylinder and cone in doSamplesVerifyModel, through an intermediate local method, which seems superfluous?

So a question again arise, should one assume that a direction is normalized? Its seems so in the cylinder model coefficients as it gets normalized, but should or should we not add that assumption in general - and update the documentation accordingly?

Not sure if I understand you correctly - the direction of the cylinder axis gets normalized (e.g. in countWithinDistance), so there is no assumption that the user passed in model coefficients with the direction already normalized. I would keep it that way since it is more robust, and normalizing the axis direction once costs very little compared to the rest of the function. However I also do not really see the need to explicitly say in the documentation that the direction does not have to be normalized? I works either way.

I was generally taking about the sqrPointToLineDistance function in common. It doesn't tell in the docs if the line_dir should be normalized. But if we change to the dot vector calculation, it has to be (or it would be required to do it in the method, but that would probably end up being slower than the cross product version.

Hope that makes more sense 😄

[akretz]

  double inline
  sqrPointToLineDistance (const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir)
  {
    // Calculate the distance from the point to the line
    // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p1-p0)) / norm(p2-p1)
    const Eigen::Vector3f diff = (line_pt - pt).head<3>();           // Used in both methods (3 sub)

    const auto crossProduct = line_dir.cross3(diff);                               // 6 mul + 3 sub
    const auto crossProductNormSquared = crossProduct.squaredNorm();               // 3 mul + 2 add
    const auto lineDirNormSquared = line_dir.squaredNorm();                        // 3 mul + 2 add
    const auto crossDistanceResult = crossProductNormSquared / lineDirNormSquared; // 1 div

    // operation sum: 12 mul + 4 add + 3 sub + 1 div

    // Optimized form: ||diff||^2 - (diff·dir)^2 / ||dir||^2
    const Eigen::Vector3f dir = line_dir.head<3>();
    const float dir_len_sq = dir.squaredNorm();             // 3 mul + 2 add
    if (dir_len_sq <= std::numeric_limits<float>::epsilon())
      return 0.0; // Degenerate direction; previous implementation would have produced
                  // inf/NaN
    const float proj = diff.dot(dir);                       // 3 mul + 2 add
    const auto diffSquaredNorm = diff.squaredNorm();        // 3 mul + 2 add
    return static_cast<double>(diffSquaredNorm -
                               (proj * proj) / dir_len_sq); // 1 mul + 1 div + 1 sub
    // operation sum: 10 mul + 6 add + 1 sub + 1 div
  }

I have splitted the operations and I get this count - so its less operations? And I think ie. a dot product can fit into a FMA operations, which could result in faster code?

I think you are right and I'm counting the same numbers of operations. You are using Lagrange's identity here. The expression I have eventually implemented in this PR is a different one, because we not only need the distance but also the direction vector. So I have done something like this to get the distance, where we get the direction vector for free inside the norm:

|| diff - diff.dot(dir) * (dir / ||dir||^2) ||^2

All three expressions should be mathematically equivalent. Not sure about numerical stability though. AI told me the cross product is most numerically stable and the expression using Lagrange's identity is the least numerically stable, because when both vectors are almost parallel, both operands of the subtraction almost cancel out.

This PR escalated quite a bit. That wasn't my intention, sorry 🫠

[larshg]

Ahhh. I thought I was using the formula from @mvieth link. Ie. like this:

    // second way - requires normalized dir for correct result, but also for performance
    const Eigen::Vector4f dir = line_dir.normalized(); // 3 mul 2 add and 3 div
    const float proj = diff.dot(dir); // 3 mul + 2 add
    const auto distDot = (diff - proj * dir).squaredNorm(); // 1 mul + 1 sub + (3 mul + 2 add - squaredNorm)
    // result 0.499999970 - the two other methods return 0.5 exactly
    // operation sum: 10 mul 6 add 3 div 1 sub

But I think I got ChatGPT to come up with an optimization, instead of writing my self, which ended up in a new way to do it.

So I guess there are 3 ways to do it.

1. Cross product = "slowest" but more stable and don't require normalized axis dir
2. Vector formulation distance function = Is faster if the axis dir is normalized - else it won't work nor be faster due to the normalizing of axis_dir
3. I guess its more a rewrite of vector formulation distance function = faster than cross product and doesn't require normalized axis_dir, but might be more unstable than cross product

This PR escalated quite a bit. That wasn't my intention, sorry 🫠

Don't be - its nice to have a good look and eventual find some optimizations 👍 And I got some math refreshed!

[mvieth]

@akretz Thanks!

@larshg Perhaps the optimization of sqrPointToLineDistance is something we should discuss in a separate PR, since it is not directly related to the changes here? I think we need at least a benchmark for that, and also a deeper look into the numerical stability.
Also just in case you are not aware: there are two sqrPointToLineDistance functions, one takes the squared length of the line direction to avoid one squaredNorm()

[larshg]

@akretz Thanks!

@larshg Perhaps the optimization of sqrPointToLineDistance is something we should discuss in a separate PR, since it is not directly related to the changes here? I think we need at least a benchmark for that, and also a deeper look into the numerical stability. Also just in case you are not aware: there are two sqrPointToLineDistance functions, one takes the squared length of the line direction to avoid one squaredNorm()

Yes, to both. It just seems to only be used in cylinder and cone, so kinda related, but it can be looked at in another PR 😄

[larshg]

@akretz can you update the timings in top message?