[Toon-members] TooN se3.h

[Ethan Eade]

CVSROOT:        /cvsroot/toon
Module name:    TooN
Changes by:     Ethan Eade <ethaneade>  08/06/30 17:58:25

Modified files:
        .              : se3.h 

Log message:
        Added standalone functions to compute transformations and their 
Jacobians
        with euclidean and inverse-depth points.

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

Patches:
Index: se3.h
===================================================================
RCS file: /cvsroot/toon/TooN/se3.h,v
retrieving revision 1.16
retrieving revision 1.17
diff -u -b -r1.16 -r1.17
--- se3.h       22 Jul 2007 02:02:17 -0000      1.16
+++ se3.h       30 Jun 2008 17:58:25 -0000      1.17
@@ -30,7 +30,7 @@
   friend SE3 operator*(const SO3& lhs, const SE3& rhs);
   friend std::istream& operator>> (std::istream& is, SE3& rhs);
   
- public:
+public:
   inline SE3();
   template <class A> inline SE3(const SO3& R, const FixedVector<3,A>& T) : 
my_rotation(R), my_translation(T) {}
       
@@ -65,9 +65,115 @@
   template <class Accessor>
   inline void trinvadjoint(FixedMatrix<6,6,Accessor>& M)const;
 
+private:
+    SO3 my_rotation;
+    Vector<3> my_translation;
+};
+
+    template <class A> inline
+    Vector<3> transform(const SE3& pose, const FixedVector<3,A>& x) { return 
pose.get_rotation()*x + pose.get_translation(); }
+    
+    template <class A> inline
+    Vector<3> transform_inverse_depth(const SE3& pose, const FixedVector<3,A>& 
uvq)
+    {
+       const Matrix<3>& R = pose.get_rotation().get_matrix();  
+       const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template 
slice<0,2>() + R.T()[2] + uvq[2] * pose.get_translation();
+       const double inv_z = 1.0/ DqT[2];
+       return makeVector(DqT[0]*inv_z, DqT[1]*inv_z, uvq[2]*inv_z);
+    }
+
+    template <class A1, class A2> inline
+    Vector<3> transform(const SE3& pose, const FixedVector<3,A1>& x, 
FixedMatrix<3,3,A2>& J_x)
+    {
+       J_x = pose.get_rotation().get_matrix();
+       return pose * x;
+    }
+
+
+    template <class A1, class A2> inline
+    Vector<3> inverse_depth_point_jacobian(const SE3& pose, const double q, 
const FixedVector<3,A1>& DqT, const double inv_z, FixedMatrix<3,3,A2>& J_uvq)
+    {
+       const Matrix<3>& R = pose.get_rotation().get_matrix();  
+       const Vector<3>& T = pose.get_translation();
+       const double u1 = DqT[0] * inv_z;
+       J_uvq[0][0] = inv_z * (R[0][0] - u1*R[2][0]);
+       J_uvq[0][1] = inv_z * (R[0][1] - u1*R[2][1]);
+       J_uvq[0][2] = inv_z * (T[0] - u1 * T[2]);
+       
+       const double v1 = DqT[1] * inv_z;
+       J_uvq[1][0] = inv_z * (R[1][0] - v1*R[2][0]);
+       J_uvq[1][1] = inv_z * (R[1][1] - v1*R[2][1]);
+       J_uvq[1][2] = inv_z * (T[1] - v1 * T[2]);
+       
+       const double q1 = q * inv_z;
+       J_uvq[2][0] = inv_z * (-q1 * R[2][0]);
+       J_uvq[2][1] = inv_z * (-q1 * R[2][1]);
+       J_uvq[2][2] = inv_z * (1.0 - q1 * T[2]);
+
+       return makeVector(u1, v1, q1);
+    }
+    
+
+    template <class A1, class A2> inline
+    Vector<3> transform_inverse_depth(const SE3& pose, const 
FixedVector<3,A1>& uvq, FixedMatrix<3,3,A2>& J_uvq)
+    {
+       const Matrix<3>& R = pose.get_rotation().get_matrix();  
+       const Vector<3>& T = pose.get_translation();
+       const double q = uvq[2];
+       const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template 
slice<0,2>() + R.T()[2] + q * T;
+       const double inv_z = 1.0 / DqT[2];
+
+       return inverse_depth_point_jacobian(pose, q, DqT, inv_z, J_uvq);
+    }
 
-    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 
+    template <class A1, class A2, class A3> inline
+    Vector<3> transform(const SE3& pose, const FixedVector<3,A1>& x, 
FixedMatrix<3,3,A2>& J_x, FixedMatrix<3,6,A3>& J_pose)
+    {
+       J_x = pose.get_rotation().get_matrix();
+       Identity(J_pose.template slice<0,0,3,3>());
+       const Vector<3> cx = pose * x;
+       J_pose[0][3] = J_pose[1][4] = J_pose[2][5] = 0;
+       J_pose[1][3] = -(J_pose[0][4] = cx[2]);
+       J_pose[0][5] = -(J_pose[2][3] = cx[1]);
+       J_pose[2][4] = -(J_pose[1][5] = cx[0]);
+       return cx;
+    }
+
+    template <class A1, class A2, class A3> inline
+    Vector<3> transform_inverse_depth(const SE3& pose, const 
FixedVector<3,A1>& uvq, FixedMatrix<3,3,A2>& J_uvq, FixedMatrix<3,6,A3>& J_pose)
+    {
+       const Matrix<3>& R = pose.get_rotation().get_matrix();  
+       const Vector<3>& T = pose.get_translation();
+       const double q = uvq[2];
+       const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template 
slice<0,2>() + R.T()[2] + q * T;
+       const double inv_z = 1.0 / DqT[2];
+       
+       const Vector<3> uvq1 = inverse_depth_point_jacobian(pose, q, DqT, 
inv_z, J_uvq);
+       const double q1 = uvq1[2];
+
+       J_pose[0][1] = J_pose[1][0] = J_pose[2][0] = J_pose[2][1] = 0;
+       J_pose[0][0] = J_pose[1][1] = q1;
+       J_pose[0][2] = -uvq1[0] * q1;
+       J_pose[1][2] = -uvq1[1] * q1;
+       J_pose[2][2] = -q1 * q1;
+       
+       J_pose[0][3] = -uvq1[1]*uvq1[0];
+       J_pose[0][4] = 1 + uvq1[0]*uvq1[0];
+       J_pose[0][5] = -uvq1[1];
+       
+       J_pose[1][3] = -1 - uvq1[1]*uvq1[1];
+       J_pose[1][4] = uvq1[0] * uvq1[1];
+       J_pose[1][5] = uvq1[0];
+
+       J_pose[2][3] = -uvq1[1]*q1;
+       J_pose[2][4] = uvq1[0]*q1;
+       J_pose[2][5] = 0;
+
+       return uvq1;
+    }
+
+    template <class A1, class A2, class A3> inline
+    Vector<2> project_transformed_point(const SE3& pose, const 
FixedVector<3,A1>& in_frame, FixedMatrix<2,3,A2>& J_x, FixedMatrix<2,6,A3>& 
J_pose)
     {
        const double z_inv = 1.0/in_frame[2];
        const double x_z_inv = in_frame[0]*z_inv;
@@ -84,7 +190,7 @@
        J_pose[1][4] =  cross;
        J_pose[1][5] =  x_z_inv;    
        
-       const TooN::Matrix<3>& R = get_rotation().get_matrix();
+       const TooN::Matrix<3>& R = pose.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]);
@@ -92,58 +198,45 @@
        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;
+       return makeVector(x_z_inv, y_z_inv);
     }
 
-    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 A1> inline
+    Vector<2> transform_and_project(const SE3& pose, const FixedVector<3,A1>& 
x)
+    {
+       return project(transform(pose,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> inline
+    Vector<2> transform_and_project_inverse_depth(const SE3& pose, const 
FixedVector<3,A1>& uvq)
+    {
+       const Matrix<3>& R = pose.get_rotation().get_matrix();  
+       const Vector<3>& T = pose.get_translation();
+       const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template 
slice<0,2>() + R.T()[2] + uvq[2] * T;
+       return project(DqT);
     }
 
-    
-    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 
+    template <class A1, class A2, class A3> inline
+    Vector<2> transform_and_project(const SE3& pose, const FixedVector<3,A1>& 
x, FixedMatrix<2,3,A2>& J_x, FixedMatrix<2,6,A3>& J_pose)
     {
-       return project_transformed_point(*this * x, J_x, J_pose);
+       return project_transformed_point(pose, transform(pose,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
+    template <class A1, class A2, class A3> inline
+    Vector<2> transform_and_project_inverse_depth(const SE3& pose, const 
FixedVector<3,A1>& uvq, FixedMatrix<2,3,A2>& J_uvq, FixedMatrix<2,6,A3>& J_pose)
     {
-       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];
+       const Matrix<3>& R = pose.get_rotation().get_matrix();  
+       const Vector<3>& T = pose.get_translation();
+       const double q = uvq[2];
+       const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template 
slice<0,2>() + R.T()[2] + q * T;
+       const Vector<2> uv = project_transformed_point(pose, DqT, J_uvq, 
J_pose);
+       J_uvq.T()[2] = J_pose.template slice<0,0,2,3>() * 
pose.get_translation();
+       J_pose.template slice<0,0,2,3>() *= q;
        return uv;
     }
 
-private:
-  SO3 my_rotation;
-  Vector<3> my_translation;
-};
+
 
 
 // left multiply an SE3 by an SO3 
@@ -235,7 +328,7 @@
 
 template <class Accessor> inline
 Vector<3> operator*(const SE3& lhs, const FixedVector<3,Accessor>& rhs){
-    return lhs.transform(rhs);
+    return lhs.get_rotation()*rhs + lhs.get_translation();
 }
 
 
@@ -350,8 +443,7 @@
     return result;
 }
 
-
- inline Vector<6> SE3::ln(const SE3& se3) {
+inline Vector<6> SE3::ln(const SE3& se3) {
     Vector<3> rot = se3.my_rotation.ln();
     double theta = sqrt(rot*rot);
     double shtot = 0.5;