First version 3D of TF roughly completed and prior to revision

src/tf.h

@@ -12,13 +12,21 @@
 
 #include "wolf.h"
 
+/**
+ * Class for 3D frame transforms.
+ *
+ * This is a simple class to manage frame updates so that rotation and homogeneous matrices
+ * are computed only once for the lifetime of a specific TF parametrization.
+ *
+ * Unit quaternions are used for 3D orientation input. They require the real part at the end.
+ */
 class TF
 {
     private:
         unsigned int epoch_; ///< epoch counter to control updates automatically
 
     protected:
-        Eigen::Vector3s t_; ///< Position vector
+        Eigen::Vector3s p_; ///< Position vector
         Eigen::Vector4s q_vector_; ///< Orientation quaternion in vector form
         Eigen::Quaternions q_; ///< Orientation quaternion in quaternion form
         Eigen::Matrix3s R_; ///< Rotation matrix
@@ -27,11 +35,11 @@ class TF
     public:
 
         /**
-         * Default constructor
+         * Default constructor to the origin, P=(0,0,0), Q=(0,0,0,1), R=identity, H=identity
          */
         TF() : epoch_(0),
-               t_(Eigen::Vector3s::Zero()),
-               q_vector_(0,0,0,0), // Impose a non-valid quaternion so that it will be updated for sure. This prevents a bad initialization due to different convention in the order of components.
+               p_(Eigen::Vector3s::Zero()),
+               q_vector_(0,0,0,1), // Impose an identity quaternion with real part at the end. This is Eigen compatible. TODO: check components order
                q_(Eigen::Quaternions::Identity()),
                R_(Eigen::Matrix3s::Identity()),
                H_(Eigen::Matrix4s::Identity())
@@ -39,75 +47,82 @@ class TF
     }
 
         /**
-         *
+         * Initializer constuctor
+         * \param _p_ptr pointer to position vector
+         * \param _q_vec_ptr pointer to quaternion vector (real part at the end)
          */
-        TF(double* _t_ptr, double* _q_ptr) :
+        TF(double* _p_ptr, double* _q_vec_ptr) :
             epoch_(0),
-            t_(_t_ptr[0], _t_ptr[1], _t_ptr[2]),
-            q_vector_(_q_ptr[0],_q_ptr[1],_q_ptr[2],_q_ptr[3]), //TODO: check components order!
-            q_(_q_ptr), //TODO: check components order!
+            p_(_p_ptr[0], _p_ptr[1], _p_ptr[2]),
+            q_vector_(_q_vec_ptr[0],_q_vec_ptr[1],_q_vec_ptr[2],_q_vec_ptr[3]), //TODO: check components order!
+            q_(_q_vec_ptr), //TODO: check components order!
             R_(q_.matrix()),
             H_(Eigen::Matrix4s::Identity())
         {
             H_.block<3,3>(0,0) = R_;
-            H_.block<3,1>(0,3) = t_;
+            H_.block<3,1>(0,3) = p_;
         }
 
     public:
-        void update(double* _t, double* _q_vector)
+        void checkAndUpdate(double* _p_ptr, double* _q_vec_ptr)
         {
-            if (!equalT(_t))
-            {
-                t_(0) = _t[0];
-                t_(1) = _t[1];
-                t_(2) = _t[2];
-                H_.block<3,1>(0,3) = t_;
-            }
-            if (!equalQ(_q_vector))
-            {
-                q_ = Eigen::Quaternion(_q_vector);
-                R_ = q_.matrix();
-                H_.block<3,3>(0,0) = R_;
-            }
+            if (!equalP(_p_ptr))
+                updateP(_p_ptr);
+            if (!equalQ(_q_vec_ptr))
+                updateO(_q_vec_ptr);
         }
 
-        void update(double* _t, double* _q_vector, unsigned int _epoch)
+        void checkAndUpdate(double* _p_ptr, double* _q_vec_ptr, unsigned int _epoch)
         {
             if(_epoch != epoch_)
             {
                 epoch_ = _epoch;
-                t_(0) = _t[0];
-                t_(1) = _t[1];
-                t_(2) = _t[2];
-                q_ = Eigen::Quaternion(_q_vector);
-                R_ = q_.matrix();
-                H_.block<3,3>(0,0) = R_;
-                H_.block<3,1>(0,3) = t_;
+                updateP(_p_ptr);
+                updateO(_q_vec_ptr);
             }
         }
 
 
     private:
-        bool equalT(double * _t)
+        /** Check if P has changed
+         *
+         */
+        bool equalP(double * _p_ptr)
         {
-            return ((_t[0] == t_(0)) && (_t[1] == t_(1)) && (_t[2] == t_(2)));
+            return ((_p_ptr[0] == p_(0)) && (_p_ptr[1] == p_(1)) && (_p_ptr[2] == p_(2)));
         }
-        bool equalQ(double * _q)
+
+        /** Check if Q has changed
+         *
+         */
+        bool equalQ(double * _q_vec_ptr)
         {
-            return ((_q[0] == q_vector_(0)) && (_q[1] == q_vector_(1)) && (_q[2] == q_vector_(2)) && (_q[3] == q_vector_(3)));
+            return ((_q_vec_ptr[0] == q_vector_(0)) && (_q_vec_ptr[1] == q_vector_(1)) && (_q_vec_ptr[2] == q_vector_(2)) && (_q_vec_ptr[3] == q_vector_(3)));
         }
-        void updateT(double * _t)
+
+        /** Update translation vector
+         *
+         */
+        void updateP(double * _p_ptr)
         {
-            t_(0) = _t[0];
-            t_(1) = _t[1];
-            t_(2) = _t[2];
+            p_(0) = _p_ptr[0];
+            p_(1) = _p_ptr[1];
+            p_(2) = _p_ptr[2];
+            H_.block<3,1>(0,3) = p_;
         }
-        void updateO(double * _q_vector)
+
+        /** Update orientation parts
+         *
+         */
+        void updateO(double * _q_vec_ptr)
         {
-            q_ = Eigen::Quaternion(_q_vector);
+            q_vector_(0) = _q_vec_ptr[0];
+            q_vector_(1) = _q_vec_ptr[1];
+            q_vector_(2) = _q_vec_ptr[2];
+            q_vector_(3) = _q_vec_ptr[3];
+            q_ = Eigen::Quaternion(_q_vec_ptr);
             R_ = q_.matrix();
             H_.block<3,3>(0,0) = R_;
-            H_.block<3,1>(0,3) = t_;
         }
 
 };