Feature/se3vel rebased (#57)

* Initial commit for  SE3vel class, the extended SE3 group with velocity coordinates

* SE3velCov class for compounding initial commit #72

* new line at the EOF

* brackets function in namespace mrob, visible for all other files including SE3cov

* brackets function now defined in SE3cov and used in other files, such as SE3velCov

* test commit

* headers to new classes

* changing convention to SE3vel to be rot, trans, vel

* updating convention on Se3vel and moving common left jacobian to SE3

* python bindings to SE3vel. Needs example.

* adding to string pythong bindings for the SE3 vel

* SE3 vel fix of Ln

* python bindings to SE3vel adding test, not working now

* SE3 vel adding inverse. SE3vel covariance not working. Ready to release (for SE3vel).

---------

Co-authored-by: Aleksei Panchenko <nosmokingsurfer@gmail.com>

mrobpy/tests/se3_vel_test.py

@@ -0,0 +1,64 @@
+from mrob import SE3vel, SO3
+import numpy as np
+
+# TODO, add proper test, this is just initial try
+
+# -------------------------------------------------------------------------
+# 1 Testing contructors and print
+# -------------------------------------------------------------------------
+T = SE3vel()
+print(T)
+
+
+theta = np.random.randn(3)
+R = SO3(theta)
+print(R)
+T1 = SE3vel(R, 0*np.random.randn(3), np.random.randn(3))
+T1.print()
+
+
+xi = np.random.randn(9)
+print('xi initializator:\n', xi)
+T = SE3vel(xi)
+T.print()
+
+
+# -------------------------------------------------------------------------
+# 2 Testing logarithm exponent
+# -------------------------------------------------------------------------
+xi_reprojected = T.Ln()
+print('xi reprojected:\n', xi_reprojected)
+print('matrix proof:\n', SE3vel(xi_reprojected))
+
+
+
+# -------------------------------------------------------------------------
+# 2 Testing extracting data from T
+# -------------------------------------------------------------------------
+print('full matrix: \n', T.T())
+print('rotation: \n', T.R())
+print('translation: \n', T.t())
+print('velocity: \n', T.v())
+print('full matrix compact: \n', T.T_compact())
+
+
+
+
+
+# -------------------------------------------------------------------------
+# 4 Testing Operations
+# -------------------------------------------------------------------------
+print('testing inverse', T1*T1.inv())
+
+
+
+# -------------------------------------------------------------------------
+# 3 Testing Adjoint
+# -------------------------------------------------------------------------
+#print(T.adj())
+# create a T exp(xi) = Exp( adj_T xi ) T
+xi = np.random.randn(9)
+print(T * SE3vel(xi))
+print(SE3vel(T.adj() @ xi) * T)
+
+

src/common/mrob/matrix_base.hpp

@@ -47,6 +47,7 @@ using Mat3 = Eigen::Matrix<matData_t, 3,3, Eigen::RowMajor>;
 using Mat4 = Eigen::Matrix<matData_t, 4,4, Eigen::RowMajor>;
 using Mat5 = Eigen::Matrix<matData_t, 5,5, Eigen::RowMajor>;
 using Mat6 = Eigen::Matrix<matData_t, 6,6, Eigen::RowMajor>;
+using Mat9 = Eigen::Matrix<matData_t, 9,9, Eigen::RowMajor>;
 using MatX = Eigen::Matrix<matData_t, Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor>;
 
 //Sparse Matrices
@@ -60,6 +61,7 @@ using Mat31 = Eigen::Matrix<matData_t, 3,1>;
 using Mat41 = Eigen::Matrix<matData_t, 4,1>;
 using Mat51 = Eigen::Matrix<matData_t, 5,1>;
 using Mat61 = Eigen::Matrix<matData_t, 6,1>;
+using Mat91 = Eigen::Matrix<matData_t, 9,1>;
 using MatX1 = Eigen::Matrix<matData_t, Eigen::Dynamic,1>;
 
 // Definition of row matrices (vectors)
@@ -68,6 +70,7 @@ using Mat13 = Eigen::Matrix<matData_t, 1,3>;
 using Mat14 = Eigen::Matrix<matData_t, 1,4>;
 using Mat15 = Eigen::Matrix<matData_t, 1,5>;
 using Mat16 = Eigen::Matrix<matData_t, 1,6>;
+using Mat19 = Eigen::Matrix<matData_t, 1,9>;
 using Mat1X = Eigen::Matrix<matData_t, 1,Eigen::Dynamic>;
 
 

src/geometry/CMakeLists.txt

@@ -5,13 +5,17 @@ SET(sources
     SO3.cpp
     SE3.cpp
     SE3cov.cpp
+    SE3vel.cpp
+    SE3velCov.cpp
 )
 
 # extra header files
 SET(headers
     mrob/SO3.hpp
     mrob/SE3.hpp
     mrob/SE3cov.hpp
+    mrob/SE3vel.hpp
+    mrob/SE3velCov.hpp
 )
 
 # create library

src/geometry/SE3.cpp

@@ -118,28 +118,10 @@ void SE3::exp(const Mat4 &xi_hat)
     Mat31 w = xi.head<3>();
     Mat31 v = xi.tail<3>();
     SO3 rotation(w);
-    Mat3 w_hat = xi_hat.topLeftCorner<3,3>();
+    //Mat3 w_hat = xi_hat.topLeftCorner<3,3>();
 
     // Calculate the closed form of V
-    // V = I + c2*(w^) + c3*(w^)^2   ,
-    // where o = norm(w), c2 = (1 - cos(o))/o^2, c3 = (o- sin(o) / o^3
-    Mat3 V = Mat3::Identity();
-    double o = w.norm();
-    double o2 = w.squaredNorm();
-    // If rotation is not zero
-    matData_t c2, c3;
-    if ( o > 1e-3){ // c2 and c3 become numerically imprecise for o < 1-5, so we choose a conservative threshold 1e-3
-        c2 = (1 - std::cos(o))/o2;
-        c3 = (o - std::sin(o))/o2/o;
-    }
-    else
-    {
-        // second order Taylor (first order is zero since this is an even function)
-        c2 = 0.5 - o2/24;
-        // Second order Taylor
-        c3 = 1.0/6.0 - o2/120;
-    }
-    V += c2*w_hat + c3*w_hat*w_hat;
+    Mat3 V = left_jacobian(w);
 
     // Calculate the translation component t = Vv
     Mat31 t = V*v;
@@ -155,29 +137,10 @@ Mat4 SE3::ln(void) const
 {
     SO3 rotation(this->R());
     // Logarithmic mapping of the rotations
-    double o; // This is absolute value of angle
-    Mat3 w_hat = rotation.ln(&o);
+    Mat3 w_hat = rotation.ln();
 
     // calculate v = V^1 t
-    // V^-1 = I - 0.5w^ + k1 (w^)^2
-    // k1 = 1/o^2 * (1 - c1/(2c2) ) ,    c1 =sin(o)/o and c2 = (1 - cos(o))/o^2 from so3_exp
-    Mat3 Vinv = Mat3::Identity();
-    double k1;
-    // 5e-3 bound provided on numerical_test.cpp, smaller than this k1 becomes degradated
-    if (o > 5e-3)
-    {
-        double c1 = std::sin(o); //sin(o)/o, we remove the o in both coeficients
-        double c2 = (1 - std::cos(o))/o; // (1 - std::cos(o))/o/o
-        k1 = 1/o/o*(1 - 0.5*c1/c2);
-    }
-    //Taylor expansion for small o.
-    else
-    {
-        // f(o) = 1/12 + 1/2*f''*o^2
-        // f'' = 1/360
-        k1 = 1.0/12 + o*o/720;
-    }
-    Vinv += -0.5*w_hat + k1* w_hat*w_hat;
+    Mat3 Vinv = inv_left_jacobian(vee3(w_hat));
 
     // v = V^-1 t
     Mat31 v = Vinv * T_.topRightCorner<3,1>();
@@ -303,6 +266,59 @@ Mat41 SE3::transform_plane(const Mat41 &pi)
     return this->inv().T().transpose() * pi;
 }
 
+
+Mat3 mrob::left_jacobian(const Mat31& phi)
+{
+    Mat3 V = Mat3::Identity();
+    Mat3 phi_hat = hat3(phi);
+    double o = phi.norm();
+    double o2 = phi.squaredNorm();
+    // If rotation is not zero
+    matData_t c2, c3;
+    if ( o > 1e-3){ // c2 and c3 become numerically imprecise for o < 1-5, so we choose a conservative threshold 1e-3
+        c2 = (1 - std::cos(o))/o2;
+        c3 = (o - std::sin(o))/o2/o;
+    }
+    else
+    {
+        // second order Taylor (first order is zero since this is an even function)
+        c2 = 0.5 - o2/24;
+        // Second order Taylor
+        c3 = 1.0/6.0 - o2/120;
+    }
+    V += c2*phi_hat + c3*phi_hat*phi_hat;
+
+    return V;
+}
+
+
+Mat3 mrob::inv_left_jacobian(const Mat31 &phi)
+{
+    Mat3 Vinv = Mat3::Identity();
+    double k1;
+    double o = phi.norm();
+    double o2 = phi.squaredNorm();
+    Mat3 phi_hat = hat3(phi);
+    // 5e-3 bound provided on numerical_test.cpp, smaller than this k1 becomes degradated
+    if (o > 5e-3)
+    {
+        double c1 = std::sin(o); //sin(o)/o, we remove the o in both coeficients
+        double c2 = (1 - std::cos(o))/o; // (1 - std::cos(o))/o/o
+        k1 = 1/o2*(1 - 0.5*c1/c2);
+    }
+    //Taylor expansion for small o.
+    else
+    {
+        // f(o) = 1/12 + 1/2*f''*o^2
+        // f'' = 1/360
+        k1 = 1.0/12 + o2/720;
+    }
+    Vinv += -0.5*phi_hat + k1* phi_hat*phi_hat;
+
+    // v = V^-1 t
+    return Vinv;
+}
+
 // Here we deine a global variable inside the file of this class, to be copied
 const std::vector<Mat4> LieGenerative{
 

src/geometry/SE3cov.cpp

@@ -33,12 +33,12 @@ SE3Cov::SE3Cov(const SE3& T, const Mat6 &cov): SE3(T), covariance_(cov){}
 
 SE3Cov::SE3Cov(const SE3Cov& pose):SE3(Mat4(pose.T())), covariance_(pose.cov()){}
 
-Mat3 brackets(const Mat3& A)
+Mat3 mrob::brackets(const Mat3& A)
 {
     return -A.trace()*Mat3::Identity() + A;
 }
 
-Mat3 brackets(const Mat3& A, const Mat3& B)
+Mat3 mrob::brackets(const Mat3& A, const Mat3& B)
 {
     return brackets(A)*brackets(B) + brackets(B*A);
 }