Merge pull request #43 from ksyy/master

Corrected the inverse jacobian computing

Author: Olivier Stasse

src/feature/feature-point6d.cpp

@@ -376,16 +376,18 @@ void FeaturePoint6d::inverseJacobianRodrigues ()
       if (norm_2 < accuracy_) {
 	P_.setIdentity ();
       }
-      // This code has been generated by maxima software
-      P_ (0,0) = ((r3_2+r2_2)*sqrt(norm_2)*sin(sqrt(norm_2))+r1_2*r3_2+r1_2*r2_2+r1_4)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
-      P_ (0,1) = -(r1*r2*sqrt(norm_2)*sin(sqrt(norm_2))+(r3_3+(r2_2+r1_2)*r3)*cos(sqrt(norm_2))-r3_3-r1*r2*r3_2+(-r2_2-r1_2)*r3-r1*r2_3-r1_3*r2)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
-      P_ (0,2) = -(r1*r3*sqrt(norm_2)*sin(sqrt(norm_2))+(-r2*r3_2-r2_3-r1_2*r2)*cos(sqrt(norm_2))-r1*r3_3+r2*r3_2+(-r1*r2_2-r1_3)*r3+r2_3+r1_2*r2)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
-      P_ (1,0) = -(r1*r2*sqrt(norm_2)*sin(sqrt(norm_2))+((-r2_2-r1_2)*r3-r3_3)*cos(sqrt(norm_2))+r3_3-r1*r2*r3_2+(r2_2+r1_2)*r3-r1*r2_3-r1_3*r2)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
-      P_ (1,1) = ((r3_2+r1_2)*sqrt(norm_2)*sin(sqrt(norm_2))+r2_2*r3_2+r2_4+r1_2*r2_2)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
-      P_ (1,2) = -(r2*r3*sqrt(norm_2)*sin(sqrt(norm_2))+(r1*r3_2+r1*r2_2+r1_3)*cos(sqrt(norm_2))-r2*r3_3-r1*r3_2+(-r2_3-r1_2*r2)*r3-r1*r2_2-r1_3)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
-      P_ (2,0) = -(r1*r3*sqrt(norm_2)*sin(sqrt(norm_2))+(r2*r3_2+r2_3+r1_2*r2)*cos(sqrt(norm_2))-r1*r3_3-r2*r3_2+(-r1*r2_2-r1_3)*r3-r2_3-r1_2*r2)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
-      P_ (2,1) = -(r2*r3*sqrt(norm_2)*sin(sqrt(norm_2))+(-r1*r3_2-r1*r2_2-r1_3)*cos(sqrt(norm_2))-r2*r3_3+r1*r3_2+(-r2_3-r1_2*r2)*r3+r1*r2_2+r1_3)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
-      P_ (2,2) = ((r2_2+r1_2)*sqrt(norm_2)*sin(sqrt(norm_2))+r3_4+(r2_2+r1_2)*r3_2)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
+      else {
+        // This code has been generated by maxima software
+        P_ (0,0) = ((r3_2+r2_2)*sqrt(norm_2)*sin(sqrt(norm_2))+r1_2*r3_2+r1_2*r2_2+r1_4)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
+        P_ (0,1) = -(r1*r2*sqrt(norm_2)*sin(sqrt(norm_2))+(r3_3+(r2_2+r1_2)*r3)*cos(sqrt(norm_2))-r3_3-r1*r2*r3_2+(-r2_2-r1_2)*r3-r1*r2_3-r1_3*r2)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
+        P_ (0,2) = -(r1*r3*sqrt(norm_2)*sin(sqrt(norm_2))+(-r2*r3_2-r2_3-r1_2*r2)*cos(sqrt(norm_2))-r1*r3_3+r2*r3_2+(-r1*r2_2-r1_3)*r3+r2_3+r1_2*r2)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
+        P_ (1,0) = -(r1*r2*sqrt(norm_2)*sin(sqrt(norm_2))+((-r2_2-r1_2)*r3-r3_3)*cos(sqrt(norm_2))+r3_3-r1*r2*r3_2+(r2_2+r1_2)*r3-r1*r2_3-r1_3*r2)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
+        P_ (1,1) = ((r3_2+r1_2)*sqrt(norm_2)*sin(sqrt(norm_2))+r2_2*r3_2+r2_4+r1_2*r2_2)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
+        P_ (1,2) = -(r2*r3*sqrt(norm_2)*sin(sqrt(norm_2))+(r1*r3_2+r1*r2_2+r1_3)*cos(sqrt(norm_2))-r2*r3_3-r1*r3_2+(-r2_3-r1_2*r2)*r3-r1*r2_2-r1_3)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
+        P_ (2,0) = -(r1*r3*sqrt(norm_2)*sin(sqrt(norm_2))+(r2*r3_2+r2_3+r1_2*r2)*cos(sqrt(norm_2))-r1*r3_3-r2*r3_2+(-r1*r2_2-r1_3)*r3-r2_3-r1_2*r2)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
+        P_ (2,1) = -(r2*r3*sqrt(norm_2)*sin(sqrt(norm_2))+(-r1*r3_2-r1*r2_2-r1_3)*cos(sqrt(norm_2))-r2*r3_3+r1*r3_2+(-r2_3-r1_2*r2)*r3+r1*r2_2+r1_3)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
+        P_ (2,2) = ((r2_2+r1_2)*sqrt(norm_2)*sin(sqrt(norm_2))+r3_4+(r2_2+r1_2)*r3_2)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
+      }
       Pinv_= P_.inverse ();
 }