[Toon-members] TooN so3.h se3.h

[Ethan Eade]

CVSROOT:        /cvsroot/toon
Module name:    TooN
Changes by:     Ethan Eade <ethaneade>  07/07/22 02:02:18

Modified files:
        .              : so3.h se3.h 

Log message:
        Updated SO3::exp and SE3::exp to use a more direct application of the 
Rodrigues formula.  Via careful use of Taylor expansions, this provides at 
least as much precision in all magnitudes of theta, while being 1.5 to 2 times 
as fast.
        
        Also added derivative-computing and inverse depth member functions to 
SE3:
        
        transform(x) // Map cartesian point x through the SE3, returning the 
cartesian result.
        
        transform_uvq(uvq) // Map inverse depth point uvq through the SE3, 
returning the inverse depth result.
        
        transform(x, J_x, J_pose)  // Map cartesian point x through the 
transformation, returning the cartesian result, and filling the Jacobians J_x 
and J_pose.
        
        transform_and_project(x, J_x, J_pose) // Map cartesian point x through 
the SE3, perspective-project the result and return the resulting (u,v) 
coordinates, and fill the Jacobians.
        
        transform_and_project_uvq(uvq, J_uvq, J_pose) // Map inverse depth 
point uvq through the SE3, then perspective-project the result (in this case 
truncate the resulting vector to two elements) and return the resulting (u,v) 
coordinates, and fill the Jacobians.
        
        The J_x or J_uvq Jacobians have 3 columns, and represent the change in 
the output as the input point (x or uvq) changes.
        The J_pose Jacobians have 6 columns, and represent the change in the 
output as the SE3 parameters change.  The translation parameters correspond to 
the first three columns, and the rotation parameters to the last three.  All 
derivatives are done assuming standard left multiplication of the SE3 by 
exp(delta), where delta is an element of the tangent space (a six vector).

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/so3.h?cvsroot=toon&r1=1.19&r2=1.20
http://cvs.savannah.gnu.org/viewcvs/TooN/se3.h?cvsroot=toon&r1=1.15&r2=1.16

Patches:
Index: so3.h
===================================================================
RCS file: /cvsroot/toon/TooN/so3.h,v
retrieving revision 1.19
retrieving revision 1.20
diff -u -b -r1.19 -r1.20
--- so3.h       21 May 2007 19:19:14 -0000      1.19
+++ so3.h       22 Jul 2007 02:02:17 -0000      1.20
@@ -148,105 +148,64 @@
      normalize(M[2]);
 }
 
-
-template <class Accessor> inline SO3 SO3::exp(const FixedVector<3,Accessor>& 
vect){
-  SO3 result;
-  
-  double theta = sqrt(vect*vect);
-  
-  double stot = 1;
-  double shtot = 0.5;
-
-  double ct=cos(theta);
-
-  if(theta > 0.001) {
-      stot = sin(theta)/theta;
-      shtot = sin(theta*0.5)/theta;
+template <class A1, class A2> 
+inline void rodrigues_so3_exp(const TooN::FixedVector<3,A1>& w, const double 
A, const double B, TooN::FixedMatrix<3,3,A2>& R)
+{
+    {
+       const double wx2 = w[0]*w[0];
+       const double wy2 = w[1]*w[1];
+       const double wz2 = w[2]*w[2];
+       
+       R[0][0] = 1.0 - B*(wy2 + wz2);
+       R[1][1] = 1.0 - B*(wx2 + wz2);
+       R[2][2] = 1.0 - B*(wx2 + wy2);
   }
-
-  result.my_matrix[0][0] = result.my_matrix[1][1] = result.my_matrix[2][2] = 
ct;
-  result.my_matrix[0][1] = -(result.my_matrix[1][0] = stot*vect[2]);
-  result.my_matrix[2][0] = -(result.my_matrix[0][2] = stot*vect[1]);
-  result.my_matrix[1][2] = -(result.my_matrix[2][1] = stot*vect[0]);
-
-  for(int i=0; i<3; i++){
-    for(int j=0; j<3; j++){
-      result.my_matrix[i][j] += 2*shtot*shtot*vect[i]*vect[j];
+    {
+       const double a = A*w[2];
+       const double b = B*(w[0]*w[1]);
+       R[0][1] = b - a;
+       R[1][0] = b + a;
     }
+    {
+       const double a = A*w[1];
+       const double b = B*(w[0]*w[2]);
+       R[0][2] = b + a;
+       R[2][0] = b - a;
+    }
+    {
+       const double a = A*w[0];
+       const double b = B*(w[1]*w[2]);
+       R[1][2] = b - a;
+       R[2][1] = b + a;
   }
-  return result;
 }
 
-template <class Accessor> inline double SO3::exp_with_half(const 
FixedVector<3,Accessor>& vect, SO3& first, SO3& second, double& shtot){
-     const double thetasq = vect*vect;
-     const double theta = sqrt(thetasq);
-     double stot, sqtoth, cth, ct;
-     if (thetasq < 1e-6) {
-        const double sixth_thetasq = thetasq/6.0;
-        stot = 1 - sixth_thetasq;
-        shtot = 0.5 - sixth_thetasq*0.125;
-        sqtoth = 0.5 - sixth_thetasq*0.03125;
-        cth = 1.0 - thetasq*0.125;
-        ct = 1.0 - thetasq*0.5;
-     } else {
-        const double thetainv = 1.0/theta;
-        const double ht = theta*0.5;
-        const double sth = sin(ht);
-        cth = cos(ht);
-        ct = 2*cth*cth - 1;
-        shtot = sth*thetainv;
-        stot = 2*cth*shtot;
-        sqtoth = 2*sqrt(0.5*(1 - cth))*thetainv;
-     }
+template <class Accessor> 
+inline SO3 SO3::exp(const FixedVector<3,Accessor>& w){
+    static const double one_6th = 1.0/6.0;
+    static const double one_20th = 1.0/20.0;
 
-     {
-        const double a = shtot*vect[0];
-        const double b = shtot*vect[1];
-        const double c = shtot*vect[2];
-        first.my_matrix[0][0] = ct + 2*a*a;
-        first.my_matrix[1][1] = ct + 2*b*b;
-        first.my_matrix[2][2] = ct + 2*c*c;
-
-        const double f = stot*vect[2];
-        const double cr1 = 2*a*b;
-        first.my_matrix[1][0] = f + cr1;
-        first.my_matrix[0][1] = cr1 - f;
-        
-        const double e = stot*vect[1];
-        const double cr2 = 2*a*c;
-        first.my_matrix[0][2] = e + cr2;
-        first.my_matrix[2][0] = cr2 - e;
-                
-        const double d = stot*vect[0];
-        const double cr3 = 2*b*c;
-        first.my_matrix[2][1] = d + cr3;
-        first.my_matrix[1][2] = cr3 - d;
-     }
+    SO3 result;
 
-     {
-        const double a = sqtoth*vect[0];
-        const double b = sqtoth*vect[1];
-        const double c = sqtoth*vect[2];
-        second.my_matrix[0][0] = cth + 0.5*a*a;
-        second.my_matrix[1][1] = cth + 0.5*b*b;
-        second.my_matrix[2][2] = cth + 0.5*c*c;
-
-        const double f = shtot*vect[2];
-        const double cr1 = 0.5*a*b;
-        second.my_matrix[1][0] = f + cr1;
-        second.my_matrix[0][1] = cr1 - f;
-        
-        const double e = shtot*vect[1];
-        const double cr2 = 0.5*a*c;
-        second.my_matrix[0][2] = e + cr2;
-        second.my_matrix[2][0] = cr2 - e;
-                
-        const double d = shtot*vect[0];
-        const double cr3 = 0.5*b*c;
-        second.my_matrix[2][1] = d + cr3;
-        second.my_matrix[1][2] = cr3 - d;
+    const double theta_sq = w*w;
+    const double theta = sqrt(theta_sq);
+    double A, B;
+    
+    if (theta_sq < 1e-8) {
+       A = 1.0 - one_6th * theta_sq;
+       B = 0.5;
+    } else {
+       if (theta_sq < 1e-6) {
+           B = 0.5 - 0.25 * one_6th * theta_sq;
+           A = 1.0 - theta_sq * one_6th*(1.0 - one_20th * theta_sq);
+       } else {
+           const double inv_theta = 1.0/theta;
+           A = sin(theta) * inv_theta;
+           B = (1 - cos(theta)) * (inv_theta * inv_theta);
+       }
      }
-     return theta;
+    rodrigues_so3_exp(w, A, B, result.my_matrix);
+    return result;
 }
 
 inline Vector<3> SO3::ln() const{

Index: se3.h
===================================================================
RCS file: /cvsroot/toon/TooN/se3.h,v
retrieving revision 1.15
retrieving revision 1.16
diff -u -b -r1.15 -r1.16
--- se3.h       29 Nov 2006 15:04:35 -0000      1.15
+++ se3.h       22 Jul 2007 02:02:17 -0000      1.16
@@ -65,6 +65,81 @@
   template <class Accessor>
   inline void trinvadjoint(FixedMatrix<6,6,Accessor>& M)const;
 
+
+    template <class A1, class A2, class A3> 
+    Vector<2> project_transformed_point(const FixedVector<3,A1>& in_frame, 
FixedMatrix<2,3,A2>& J_x, FixedMatrix<2,6,A3>& J_pose) const 
+    {
+       const double z_inv = 1.0/in_frame[2];
+       const double x_z_inv = in_frame[0]*z_inv;
+       const double y_z_inv = in_frame[1]*z_inv;
+       const double cross = x_z_inv * y_z_inv;
+       J_pose[0][0] = J_pose[1][1] = z_inv;
+       J_pose[0][1] = J_pose[1][0] = 0;
+       J_pose[0][2] = -x_z_inv * z_inv;
+       J_pose[1][2] = -y_z_inv * z_inv;
+       J_pose[0][3] = -cross;
+       J_pose[0][4] = 1 + x_z_inv*x_z_inv; 
+       J_pose[0][5] = -y_z_inv;  
+       J_pose[1][3] = -1 - y_z_inv*y_z_inv;
+       J_pose[1][4] =  cross;
+       J_pose[1][5] =  x_z_inv;    
+       
+       const TooN::Matrix<3>& R = get_rotation().get_matrix();
+       J_x[0][0] = z_inv*(R[0][0] - x_z_inv * R[2][0]);
+       J_x[0][1] = z_inv*(R[0][1] - x_z_inv * R[2][1]);
+       J_x[0][2] = z_inv*(R[0][2] - x_z_inv * R[2][2]);
+       J_x[1][0] = z_inv*(R[1][0] - y_z_inv * R[2][0]);
+       J_x[1][1] = z_inv*(R[1][1] - y_z_inv * R[2][1]);
+       J_x[1][2] = z_inv*(R[1][2] - y_z_inv * R[2][2]);
+       
+       Vector<2> uv;
+       uv[0] = x_z_inv;
+       uv[1] = y_z_inv;
+       return uv;
+    }
+
+    template <class A> Vector<3> 
+    transform(const FixedVector<3,A>& x) const { return my_rotation * x + 
my_translation; }
+    
+    template <class A1, class A2, class A3> 
+    Vector<3> transform(const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x, 
FixedMatrix<3,6,A3>& J_pose) const {     
+       const Vector<3> se3_x = *this * x;
+       J_x = my_rotation.get_matrix();
+       Identity(J_pose.template slice<0,0,3,3>());
+       J_pose[0][3] = J_pose[1][4] = J_pose[2][5] = 0;
+       J_pose[0][4] =  se3_x[2]; J_pose[0][5] = -se3_x[1];
+       J_pose[1][3] = -se3_x[2]; J_pose[1][5] =  se3_x[0];
+       J_pose[2][3] =  se3_x[1]; J_pose[2][4] = -se3_x[0];
+       return se3_x;
+    }
+
+
+    template <class A> 
+    Vector<3> transform_uvq(const FixedVector<3,A>& uvq) const {
+       const Matrix<3>& R = my_rotation.get_matrix();  
+       const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template 
slice<0,2>() + R.T()[2] + uvq[2] * my_translation;
+       const double inv_z = 1.0/ DqT[2];
+       const double vals[3] = {DqT[0] * inv_z, DqT[1]*inv_z, uvq[2]*inv_z};
+       return Vector<3>(vals);
+    }
+
+    
+    template <class A1, class A2, class A3> 
+    Vector<2> transform_and_project(const FixedVector<3,A1>& x, 
FixedMatrix<2,3,A2>& J_x, FixedMatrix<2,6,A3>& J_pose) const 
+    {
+       return project_transformed_point(*this * x, J_x, J_pose);
+    }
+    
+    template <class A1, class A2, class A3> 
+    Vector<2> transform_and_project_uvq(const FixedVector<3,A1>& uvq, 
FixedMatrix<2,3,A2>& J_uvq, FixedMatrix<2,6,A3>& J_pose) const
+    {
+       const Vector<3> DqT = get_rotation() * TooN::unproject(uvq.template 
slice<0,2>()) + uvq[2] * get_translation();
+       const Vector<2> uv = project_transformed_point(DqT, J_uvq, J_pose);
+       J_uvq.T()[2] = J_pose.template slice<0,0,2,3>() * get_translation();
+       J_pose.template slice<0,0,2,3>() *= uvq[2];
+       return uv;
+    }
+
 private:
   SO3 my_rotation;
   Vector<3> my_translation;
@@ -160,8 +235,7 @@
 
 template <class Accessor> inline
 Vector<3> operator*(const SE3& lhs, const FixedVector<3,Accessor>& rhs){
-  Vector<3> v = lhs.get_rotation()*rhs;
-  return v + lhs.get_translation();
+    return lhs.transform(rhs);
 }
 
 
@@ -242,28 +316,41 @@
 {}
 
 
-inline SE3 SE3::exp(const Vector<6>& vect){
-  SE3 result;
-  SO3 halfrotator;
-  Vector<3> trans(vect.slice<0,3>());
-  Vector<3> rot(vect.slice<3,3>());
-
-  double shtot;
-  double theta = SO3::exp_with_half(rot, result.my_rotation, halfrotator, 
shtot);
+inline SE3 SE3::exp(const Vector<6>& mu){
+    static const double one_6th = 1.0/6.0;
+    static const double one_20th = 1.0/20.0;
   
-  result.my_translation = halfrotator * trans * (2*shtot);
+    SE3 result;
 
-  if(theta > 0.001){
-    result.my_translation += rot * ((trans * rot) * (1- 2*shtot) / (rot * 
rot));
+    const Vector<3> w = mu.slice<3,3>();
+    const double theta_sq = w*w;
+    const double theta = sqrt(theta_sq);
+    double A, B;
+    
+    const Vector<3> cross = w ^ mu.slice<0,3>();
+    if (theta_sq < 1e-8) {
+       A = 1.0 - one_6th * theta_sq;
+       B = 0.5;
+       result.get_translation() = mu.slice<0,3>() + 0.5 * cross;
+    } else {
+       double C;
+       if (theta_sq < 1e-6) {
+           C = one_6th*(1.0 - one_20th * theta_sq);
+           A = 1.0 - theta_sq * C;
+           B = 0.5 - 0.25 * one_6th * theta_sq;
   } else {
-    // small angle approximation that considers first two terms in expansion 
of sin(theta/2)
-    // accurate up to 10^-15
-    result.my_translation += rot * ((trans * rot)/24);
+           const double inv_theta = 1.0/theta;
+           A = sin(theta) * inv_theta;
+           B = (1 - cos(theta)) * (inv_theta * inv_theta);
+           C = (1 - A) * (inv_theta * inv_theta);
   }
-
+       result.get_translation() = mu.slice<0,3>() + B * cross + C * (w ^ 
cross);
+    }
+    rodrigues_so3_exp(w, A, B, result.my_rotation.my_matrix);
   return result;
 }
 
+
  inline Vector<6> SE3::ln(const SE3& se3) {
     Vector<3> rot = se3.my_rotation.ln();
     double theta = sqrt(rot*rot);