pcl icon indicating copy to clipboard operation
pcl copied to clipboard

Published 20 hours ago • verified publisher icon PointCloudLibrary

[Registration] Rotation epsilon enhancement : (sin(angle))^2 instead of cos(angle)

Open KotaYonezawa opened this issue 3 years ago • 4 comments

Describe the bug

Currently, transformation rotation epsilon (maximum allowable rotation difference between two consecutive transformations) must be set as cos(angle) value.

(in registration.h)

  /** \brief Set the transformation rotation epsilon (maximum allowable rotation
   * difference between two consecutive transformations) in order for an optimization to
   * be considered as having converged to the final solution. \param[in] epsilon the
   * transformation rotation epsilon in order for an optimization to be considered as
   * having converged to the final solution (epsilon is the cos(angle) in a axis-angle
   * representation).
   */
  inline void
  setTransformationRotationEpsilon(double epsilon)
  {
    transformation_rotation_epsilon_ = epsilon;
  }

But in common, cos(angle) is not good for using as threshold value around angle is near to 0. I think sin(angle) is better for it.

Context

When angle is close to 0, cos(angle) is close to 1.0, means 0.9999999.... It waists significand bits of floating point value. In addition, the slope of the Cosine function approaches 0 if angle is close to 0. It means we'll need many significand bits to distinguish cos(angle) value if angle is very close to 0.

https://en.wikipedia.org/wiki/Sine_and_cosine

Instead of cos(angle) we can easily calculate (sin(angle))^2 value from rotation matrix. I think it should be much better than cos(angle).

I modified NormalDistributionsTransform::computeTransformation() like as follows.

https://github.com/PointCloudLibrary/pcl/blob/master/registration/include/pcl/registration/impl/ndt.hpp

    delta *= delta_norm;

    // Convert delta into matrix form
    convertTransform(delta, transformation_);

    transform += delta;

    // Update Visualizer (untested)
    if (update_visualizer_)
      update_visualizer_(output, pcl::Indices(), *target_, pcl::Indices());

    const double cos_angle =
        0.5 * (transformation_.template block<3, 3>(0, 0).trace() - 1);
    const double translation_sqr =
        transformation_.template block<3, 1>(0, 3).squaredNorm();

    const double sin_angle_sqr = ((transformation_(2, 1) - transformation_(1, 2)) * (transformation_(2, 1) - transformation_(1, 2))
                                + (transformation_(0, 2) - transformation_(2, 0)) * (transformation_(0, 2) - transformation_(2, 0))
                                + (transformation_(1, 0) - transformation_(0, 1)) * (transformation_(1, 0) - transformation_(0, 1))) / 4.0;

    // FOR DEBUG
    printf ("%g %g %g %g\n", score, translation_sqr, cos_angle, sin_angle_sqr);

    nr_iterations_++;

sin_angle_sqr was added and output log to compare other values. (translation_sqr, cos_angle) This calculation formula can be easily derived from Rodrigues' Rotation Formula. (https://mathworld.wolfram.com/RodriguesRotationFormula.html)

I tried NDT matching using following example, I posted before. https://github.com/PointCloudLibrary/pcl/issues/5056#issuecomment-988864151 I changed setTransformationEpsilon to 0 to check more iteration loop.

87862.4 0.988927 0.994469 0.0110306
126489 0.0891851 0.997194 0.00560402
128543 0.0202943 0.999676 0.000648835
129053 0.00153224 0.999974 5.24327e-05
129071 6.64301e-06 1 3.91958e-07
129071 5.71179e-14 1 2.97089e-15
129071 5.65053e-14 1 2.93572e-15
129071 5.58576e-14 1 2.90353e-15
129071 5.51443e-14 1 2.86564e-15
129071 5.44038e-14 1 2.82411e-15
129071 5.37615e-14 1 2.79186e-15
129071 5.29458e-14 1 2.74898e-15
129071 5.22264e-14 1 2.70875e-15
129071 8.35703e-38 1 4.33727e-39
129071 8.35703e-38 1 4.33727e-39
129071 8.35703e-38 1 4.33727e-39
129071 8.35703e-38 1 4.33727e-39
...

cos(angle) became 1.0 in early stage, but (sin(angle))^2 can show small value. So it will be useful if the user would like to use more small rotation epsilon value, I supposed.

Your Environment (please complete the following information):

  • OS: Windows10
  • Compiler: Compiler: Visual Studio 2019 Build Tools
  • PCL Version: HEAD

KotaYonezawa avatar Mar 04 '22 09:03 KotaYonezawa

Thank you for suggesting this idea. I will investigate it further, but here are my first thoughts:

  • Can you try the printf with more digits, e.g. %.20g? Default is 6, I think. Even if the print says "1", it is probably not exactly 1, but a bit less.
  • I noticed that transformation_rotation_epsilon_ and cos_angle are stored as double, so I would not have expected that a lack of precision could be a problem
  • Did you experience a situation where you did not find an appropriate value to pass to setTransformationRotationEpsilon? Would passing 1 not be a solution, if one notices that the registration stops too early otherwise?

mvieth avatar Mar 04 '22 14:03 mvieth

Thanks a lot for your comment.

Can you try the printf with more digits, e.g. %.20g?

I tried it.

87862.35110687717679 0.98892712593078613281 0.99446940422058105469 0.011030587367713451385
126489.2696795342199 0.089185059070587158203 0.99719405174255371094 0.0056040165945887565613
128543.37856003185152 0.020294293761253356934 0.99967551231384277344 0.00064883538288995623589
129053.3929681127338 0.0015322448452934622765 0.99997377395629882812 5.2432689699344336987e-05
129070.59426767822879 6.6430129663785919547e-06 0.99999976158142089844 3.9195822409965330735e-07
129070.59510897255677 5.7117925298354188524e-14 1 2.9708871708479956536e-15
129070.59528720773233 5.6505307025186621295e-14 1 2.935722598042733375e-15
129070.59595508455823 5.5857638528661671118e-14 1 2.9035298403329128092e-15
129070.59761707991129 5.5144280933011255441e-14 1 2.8656412095692807007e-15
129070.59852671752742 5.4403825059312280188e-14 1 2.8241069490006660836e-15
129070.5994516787905 5.3761526751672922275e-14 1 2.7918608282151684968e-15
129070.60029125699657 5.2945823859071663997e-14 1 2.748984867458103068e-15
129070.60146726109087 5.2226381178728173427e-14 1 2.7087478378482084118e-15
129070.60146726109087 8.3570267872608858724e-38 1 4.3372681782119586519e-39
129070.60146726109087 8.3570267872608858724e-38 1 4.3372681782119586519e-39
129070.60146726109087 8.3570267872608858724e-38 1 4.3372681782119586519e-39
129070.60146726109087 8.3570267872608858724e-38 1 4.3372681782119586519e-39
129070.60146726109087 8.3570267872608858724e-38 1 4.3372681782119586519e-39
129070.60146726109087 8.3570267872608858724e-38 1 4.3372681782119586519e-39
...

In fact, on current code, even if cos_angle is double, transformation_ is float type matrix, so lacks precision at that time. But even if transformation_ is changed to double matrix type, wasting significand bits will be happened, I supposed.

Did you experience a situation where you did not find an appropriate value to pass to setTransformationRotationEpsilon?

Yes, I tried using some simple NDT SLAM and I found cos_angle is not so useful compared with translation_sqr for threshold.

KotaYonezawa avatar Mar 04 '22 15:03 KotaYonezawa

I am trying to weigh the possible benefits of switching from cosine to sine versus the complexity of the necessary code changes. If we switch, setTransformationRotationEpsilon has to work the same way as before, otherwise people's existing code would break. So we would probably have to add a new function where users can specify the sine of the angle. Do we also need a new member variable then or can transformation_rotation_epsilon_ serve for both uses? We would need to adapt code in registration.h, icp.hpp, ndt.hpp, ndt_2d.hpp, default_convergence_criteria.h[pp], and maybe more files. That is quite a lot to consider. Perhaps we find a more "local" solution. For example, cos_angle in ndt.hpp is computed as 0.5 * (transformation_.template block<3, 3>(0, 0).trace() - 1). Can you try the following variations in your printf to see whether they are more accurate:

  1. 0.5 * (static_cast<double>(transformation_.coeff(0, 0)) + static_cast<double>(transformation_.coeff(1, 1)) + static_cast<double>(transformation_.coeff(2, 2)) - 1.0)
  2. 0.5 * ((static_cast<double>(transformation_.coeff(0, 0)) + static_cast<double>(transformation_.coeff(1, 1)) - 1.0) + static_cast<double>(transformation_.coeff(2, 2)))
  3. (0.5*static_cast<double>(transformation_.coeff(0, 0)) + 0.5*static_cast<double>(transformation_.coeff(1, 1)) - 0.5) + 0.5*static_cast<double>(transformation_.coeff(2, 2))

Yes, I tried using some simple NDT SLAM and I found cos_angle is not so useful compared with translation_sqr for threshold.

Did the registration stop too early or what was the problem? Did you try setting transformation_rotation_epsilon_ to 0.0 or 1.0, for example?

Also possibly relevant: printing the largest double value less than 1, using nextafter: https://godbolt.org/z/oYebE7hGe

mvieth avatar Mar 05 '22 15:03 mvieth

Thanks a lot for your comment!

If we switch, setTransformationRotationEpsilon has to work the same way as before, otherwise people's existing code would break. So we would probably have to add a new function where users can specify the sine of the angle.

Yes, I agreed. If possible, current "cos_angle" should be kept, and addding new option for (sin(angle))^2 threshold will be better.

Can you try the following variations in your printf to see whether they are more accurate:

I tried like as follows.

    const double cos_angle =
        0.5 * (transformation_.template block<3, 3>(0, 0).trace() - 1);
    const double translation_sqr =
        transformation_.template block<3, 1>(0, 3).squaredNorm();

    const double sin_angle_sqr = ((transformation_(2, 1) - transformation_(1, 2)) * (transformation_(2, 1) - transformation_(1, 2))
                                + (transformation_(0, 2) - transformation_(2, 0)) * (transformation_(0, 2) - transformation_(2, 0))
                                + (transformation_(1, 0) - transformation_(0, 1)) * (transformation_(1, 0) - transformation_(0, 1))) / 4.0;

    const double cos_angle_A = 0.5 * (static_cast<double>(transformation_.coeff(0, 0))
                                    + static_cast<double>(transformation_.coeff(1, 1))
                                    + static_cast<double>(transformation_.coeff(2, 2)) - 1.0);
    const double cos_angle_B = 0.5 * ((static_cast<double>(transformation_.coeff(0, 0))
                                     + static_cast<double>(transformation_.coeff(1, 1)) - 1.0)
                                     + static_cast<double>(transformation_.coeff(2, 2)));
    const double cos_angle_C = (0.5 * static_cast<double>(transformation_.coeff(0, 0))
                              + 0.5 * static_cast<double>(transformation_.coeff(1, 1)) - 0.5)
                              + 0.5 * static_cast<double>(transformation_.coeff(2, 2));
    double cos_angle_D;
    {
        Eigen::Affine3d affine = Eigen::Translation<double, 3>(delta.head<3>()) *
            Eigen::AngleAxis<double>(delta(3), Eigen::Vector3d::UnitX()) *
            Eigen::AngleAxis<double>(delta(4), Eigen::Vector3d::UnitY()) *
            Eigen::AngleAxis<double>(delta(5), Eigen::Vector3d::UnitZ());
        Eigen::Matrix4d trans = affine.matrix();
        cos_angle_D = 0.5 * (trans.block<3, 3>(0, 0).trace() - 1);
    }

    // FOR DEBUG
    printf ("%.20g %.20g %.20g %.20g %.20g %.20g %.20g %.20g\n", score, translation_sqr, cos_angle, sin_angle_sqr,
            cos_angle_A, cos_angle_B, cos_angle_C, cos_angle_D);

Following is the result. Unfortunately, cos_angle_A, cos_angle_B, cos_angle_C seems to be almost same as original cos_angle. "cos_angle_D" (calculated using double type for matrix calculation) is better than original cos_angle. (But I still felt that (sin(angle))^2 is smarter than cos_angle_D for small angle threshold...)

87862.35110687717679 0.98892712593078613281 0.99446940422058105469 0.011030587367713451385 0.99446940422058105469 0.99446940422058105469 0.99446940422058105469 0.99446941264646060432
126489.2696795342199 0.089185059070587158203 0.99719405174255371094 0.0056040165945887565613 0.99719408154487609863 0.99719408154487609863 0.99719408154487609863 0.99719405512819214721
128543.37856003185152 0.020294293761253356934 0.99967551231384277344 0.00064883538288995623589 0.99967554211616516113 0.99967554211616516113 0.99967554211616516113 0.99967552967314698975
129053.3929681127338 0.0015322448452934622765 0.99997377395629882812 5.2432689699344336987e-05 0.99997377395629882812 0.99997377395629882812 0.99997377395629882812 0.99997378330727837437
129070.59426767822879 6.6430129663785919547e-06 0.99999976158142089844 3.9195822409965330735e-07 0.99999982118606567383 0.99999982118606567383 0.99999982118606567383 0.99999980402089461151
129070.59510897255677 5.7117925298354188524e-14 1 2.9708871708479956536e-15 1 1 1 0.99999999999999866773
129070.59528720773233 5.6505307025186621295e-14 1 2.935722598042733375e-15 1 1 1 0.99999999999999866773
129070.59595508455823 5.5857638528661671118e-14 1 2.9035298403329128092e-15 1 1 1 0.99999999999999866773
129070.59761707991129 5.5144280933011255441e-14 1 2.8656412095692807007e-15 1 1 1 0.99999999999999866773
129070.59852671752742 5.4403825059312280188e-14 1 2.8241069490006660836e-15 1 1 1 0.99999999999999866773
129070.5994516787905 5.3761526751672922275e-14 1 2.7918608282151684968e-15 1 1 1 0.99999999999999866773
129070.60029125699657 5.2945823859071663997e-14 1 2.748984867458103068e-15 1 1 1 0.99999999999999866773
129070.60146726109087 5.2226381178728173427e-14 1 2.7087478378482084118e-15 1 1 1 0.99999999999999866773
129070.60146726109087 8.3570267872608858724e-38 1 4.3372681782119586519e-39 1 1 1 1
129070.60146726109087 8.3570267872608858724e-38 1 4.3372681782119586519e-39 1 1 1 1
129070.60146726109087 8.3570267872608858724e-38 1 4.3372681782119586519e-39 1 1 1 1
129070.60146726109087 8.3570267872608858724e-38 1 4.3372681782119586519e-39 1 1 1 1
129070.60146726109087 8.3570267872608858724e-38 1 4.3372681782119586519e-39 1 1 1 1
129070.60146726109087 8.3570267872608858724e-38 1 4.3372681782119586519e-39 1 1 1 1
129070.60146726109087 8.3570267872608858724e-38 1 4.3372681782119586519e-39 1 1 1 1

Did the registration stop too early or what was the problem? Did you try setting transformation_rotation_epsilon_ to 0.0 or 1.0, for example?

I used translation_sqr = 1e-6 or more smaller in common. If I check convergence by rotation on same level accuracy, current "cos_angle" implementation is not enough.

KotaYonezawa avatar Mar 07 '22 04:03 KotaYonezawa