Rotation::GetRotAngle() returning non-zero angle and giving wrong axis when passed a small eps

[nxlou94]

Hi,
I am using KDL in the following way to obtain twist from two frames, for interpolation.

        double dt = 0.001;
        KDL::Frame P_old, P_new;
        // Some code
        KDL::Vector vel = (P_new.p - P_old.p) / dt;
        KDL::Vector axis;
        double angle = (P_new.M * P_old.M.Inverse()).GetRotAngle(axis, KDL::epsilon*dt);
        KDL::Vector rot = angle / dt * axis;
        KDL::Twist tw(vel, rot);

I notice that when P_old.M has small difference with P_new.M, I do get correct non-zero angle since I passed a smaller eps to Rotation::GetRotAngle(Vector& axis,double eps) , but I get a wrong axis, which is (1, 0, 0).
Looking into the source code, I found that Rotation::GetRotAngle(Vector& axis,double eps) does not pass eps to its function call axis.Normalize(), and as a result, passing small eps will result in times when norm of axis is smaller than KDL::epsilon, and axis.Normalize() setting axis to (1, 0, 0).
My fix is to change axis.Normalize() to axis.Normalize(0), since the normalize function do not need to check again if previous judgements in GetRotAxis() already indicates the angle big enough. This solved my problem.
However, looking more into the code, I found that the previous line angle = atan2(axis.Norm()/2,f) should also be modified to angle = atan2(axis.Norm(0)/2,f), but my problem was solved without doing that. Further looking into Vector::Norm(double eps), I found that its judgment regarding eps has logical errors, and it can return a norm smaller than KDL::epsilon but non-zero in certain situations. axis.Normalize() calls this->Norm() and noticed that norm too small, so it set axis to (1, 0, 0).

So my previous fix is necessary but not sufficient, it did solve my problem, but is not the complete solution.

In all, I discovered 3 mistakes in <frames.cpp>, and my naïve fixes are:

1. In Rotation::GetRotAxis():

   ...
   angle = atan2(axis.Norm()/2,f); // -> angle = atan2(axis.Norm(0)/2,f);
   axis.Normalize(); // -> axis.Normalize(0);
   ...

2. In Vector::Normalize():

   double Vector::Normalize(double eps) {
       double v = this->Norm(); // -> double v = this->Norm(eps);
       if (v < eps) {
           *this = Vector(1,0,0);
           return 0;
       } else {
           *this = (*this)/v;
           return v;
       }
   }

3. In Vector::Norm():

   double Vector::Norm(double eps) const
       {
           double tmp1;
           double tmp2;
           tmp1 = fabs(data[0]);
           tmp2 = fabs(data[1]);
           if (tmp1 >= tmp2) {
               tmp2=fabs(data[2]);
               if (tmp1 >= tmp2) {
                   if (tmp1 < eps) {
                       // only to everything exactly zero case, all other are handled correctly
                       return 0;
                   }
                   return tmp1*sqrt(1+sqr(data[1]/data[0])+sqr(data[2]/data[0]));
               } else {
                   if (tmp2 < eps) return 0; // <- added line
                   return tmp2*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2]));
               }
           } else {
               tmp1=fabs(data[2]);
               if (tmp2 > tmp1) {
                   if (tmp2 < eps) return 0; // <- added line
                   return tmp2*sqrt(1+sqr(data[0]/data[1])+sqr(data[2]/data[1]));
               } else {
                   if (tmp1 < eps) return 0; // <- added line
                   return tmp1*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2]));
               }
           }
       }

I roughly looked in <frames.cpp>, and noticed similar problem regarding optional eps not being passed on for Rotation2 and Vector2 as well. I think this is not intended but a bug, but I am not 100% sure.