Repository: prime-slam/mrob (language: python)

Issue:
[WIP] Fix incompatibility of current pybind and numpy 2.0 



Discussion / hints:

Fix patch:
diff --git a/src/common/mrob/matrix_base.hpp b/src/common/mrob/matrix_base.hpp
index 045518a..01bfb2d 100644
--- a/src/common/mrob/matrix_base.hpp
+++ b/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>;
 
 
diff --git a/src/geometry/CMakeLists.txt b/src/geometry/CMakeLists.txt
index 025e414..89174f6 100644
--- a/src/geometry/CMakeLists.txt
+++ b/src/geometry/CMakeLists.txt
@@ -5,6 +5,8 @@ SET(sources
     SO3.cpp
     SE3.cpp
     SE3cov.cpp
+    SE3vel.cpp
+    SE3velCov.cpp
 )
 
 # extra header files
@@ -12,6 +14,8 @@ SET(headers
     mrob/SO3.hpp
     mrob/SE3.hpp
     mrob/SE3cov.hpp
+    mrob/SE3vel.hpp
+    mrob/SE3velCov.hpp
 )
 
 # create library
diff --git a/src/geometry/SE3.cpp b/src/geometry/SE3.cpp
index b3ca08c..7fd9a64 100644
--- a/src/geometry/SE3.cpp
+++ b/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{
 
diff --git a/src/geometry/SE3cov.cpp b/src/geometry/SE3cov.cpp
index dfe4f95..f30874c 100644
--- a/src/geometry/SE3cov.cpp
+++ b/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);
 }
diff --git a/src/geometry/SE3vel.cpp b/src/geometry/SE3vel.cpp
new file mode 100644
index 0000000..9326a44
--- /dev/null
+++ b/src/geometry/SE3vel.cpp
@@ -0,0 +1,210 @@
+/* Copyright (c) 2022, Gonzalo Ferrer
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ *
+ * SE3vel.cpp
+ *
+ *  Created on: June 12, 2021
+ *      Author: Aleksei Panchenko
+ *              Gonzalo Ferrer
+ *              g.ferrer@skoltech.ru
+ *              Mobile Robotics Lab, Skoltech
+ */
+
+#include "mrob/SE3vel.hpp"
+#include "mrob/SE3.hpp"
+
+#include <iostream>
+
+using namespace mrob;
+
+SE3vel::SE3vel(const Mat5 &T) : T_(T) {}
+
+SE3vel::SE3vel(const SE3vel &T) : T_(T.T()){}
+
+SE3vel::SE3vel(const SO3 &R, const Mat31 &t, const Mat31 &v) : T_(Mat5::Identity())
+{
+    T_.topLeftCorner<3,3>() = R.R();
+    T_.block<3,1>(0,3) = t;
+    T_.topRightCorner<3,1>() = v;
+}
+
+SE3vel::SE3vel(const Mat91 &xi)
+{
+    this->Exp(xi);
+}
+ 
+Mat31 SE3vel::t() const
+{
+    return T_.block<3,1>(0,3);
+}
+
+Mat31 SE3vel::v() const
+{
+    return T_.topRightCorner<3,1>();
+}
+
+Mat3 SE3vel::R() const
+{
+    return T_.topLeftCorner<3,3>();
+}
+
+Mat5 SE3vel::T(void) const
+{
+    return this->T_;
+}
+
+Mat<3,5> SE3vel::T_compact() const
+{
+    return T_.topLeftCorner<3,5>();
+}
+
+SE3vel SE3vel::inv(void) const
+{
+    Mat5 inv(Mat5::Zero());
+    Mat3 R = this->R();
+    R.transposeInPlace();
+
+    inv << R, -R*this->t(), -R*this->v(),
+            0,0,0,1,0,
+            0,0,0,0,1;
+
+    return SE3vel(inv);
+}
+
+SE3vel SE3vel::operator*(const SE3vel& rhs)
+{
+    return SE3vel(Mat5(T_*rhs.T()));
+}
+
+SE3vel& SE3vel::operator=(const SE3vel& rhs)
+{
+    // check for self assignment TODO
+    if (this == &rhs)
+        return *this;
+    T_ = rhs.T();
+    return *this;
+}
+
+Mat9 SE3vel::adj() const
+{
+    Mat9 res(Mat9::Zero());
+    Mat3 R = this->R();
+    Mat31 v = this->v();
+    Mat31 t = this->t();
+
+    res.block<3,3>(0,0) = R;
+    res.block<3,3>(3,3) = R;
+    res.block<3,3>(6,6) = R;
+
+    res.block<3,3>(3,0) = hat3(t)*R;
+    res.block<3,3>(6,0) = hat3(v)*R;
+
+    return res;
+}
+
+Mat5 hat9(const Mat91 &xi)
+{
+    Mat5 result(Mat5::Zero());
+
+    result <<    0, -xi(2),  xi(1), xi(3), xi(6),
+             xi(2),      0, -xi(0), xi(4), xi(7),
+            -xi(1),  xi(0),      0, xi(5), xi(8),
+                 0,      0,      0,     0,     0,
+                 0,      0,      0,     0,     0;
+
+    return result;   
+}
+
+
+/**
+ * Vee operator (v), the inverse of hat
+ */
+Mat91 vee9(const Mat4 &xi_hat)
+{
+    Mat91 result(Mat91::Zero());
+
+    result << xi_hat(2,1), xi_hat(0,2), xi_hat(1,0),
+              xi_hat(0,3), xi_hat(1,3), xi_hat(2,3),
+              xi_hat(0,4), xi_hat(1,4), xi_hat(2,4);
+
+    return result;
+}
+
+
+
+void SE3vel::Exp(const Mat91& xi)  
+{
+    Mat5 result(Mat5::Identity());
+
+    Mat31 phi = xi.head(3);
+    Mat31 v = xi.segment<3>(3);
+    Mat31 t = xi.tail(3);
+
+    SO3 tmp(phi);
+
+    result.topLeftCorner<3,3>() << tmp.R();
+
+    Mat3 jac = left_jacobian(phi);
+
+    result.block<3,1>(0,3) << jac*v;
+    result.block<3,1>(0,4) << jac*t;
+
+    this->T_ = result;
+}
+
+Mat91 SE3vel::Ln() const
+{
+    Mat91 result;
+
+    Mat3 R = this->R();
+    Mat31 v = this->v();
+    Mat31 t = this->t();
+
+    SO3 tmp(R);
+    Mat31 log_R_vee = tmp.ln_vee();
+    Mat3 jac = inv_left_jacobian(log_R_vee);
+
+    result.head(3) << log_R_vee;
+    result.segment<3>(3) << jac*t;
+    result.tail(3) << jac*v;
+
+    return result;
+}
+
+void SE3vel::regenerate()
+{
+    Mat91 xi = this->Ln();
+    this->Exp(xi);
+}
+
+
+void SE3vel::print() const
+{
+    std::cout << T_ << std::endl;
+}
+
+std::ostream& SE3vel::operator<<(std::ostream &os)
+{
+    os << T_;
+    return os;
+}
+
+
+std::string SE3vel::toString() const
+{
+    std::stringstream ss;
+    ss << this->T_;
+    return ss.str();
+}
\ No newline at end of file
diff --git a/src/geometry/SE3velCov.cpp b/src/geometry/SE3velCov.cpp
new file mode 100644
index 0000000..9134653
--- /dev/null
+++ b/src/geometry/SE3velCov.cpp
@@ -0,0 +1,136 @@
+/* Copyright (c) 2022, Gonzalo Ferrer
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ *
+ * SE3velCov.cpp
+ *
+ *  Created on: June 12, 2021
+ *      Author: Aleksei Panchenko
+ *              Gonzalo Ferrer
+ *              g.ferrer@skoltech.ru
+ *              Mobile Robotics Lab, Skoltech
+ */
+
+#include "mrob/SE3velCov.hpp"
+#include <iostream>
+
+using namespace mrob;
+
+SE3velCov::SE3velCov(void)
+{
+    this->T_ = Mat5::Identity();
+    this->covariance = Mat9::Identity();
+}
+
+SE3velCov::SE3velCov(const SE3vel &pose, const Mat9 &covariance)
+{
+    this->T_ = pose.T();
+    this->covariance = covariance;
+}
+
+Mat9 SE3velCov::getQ(const Mat3 &cov_a, const Mat3 &cov_w, const double dt) const
+{
+    Mat9 Q(Mat9::Identity());
+
+    Q.topLeftCorner<3, 3>() = cov_w;
+    Q.block<3, 3>(3, 3) = cov_a;
+    Q.block<3, 3>(6, 3) = 1. / 2. * cov_a * dt;
+    Q.block<3, 3>(3, 6) = 1. / 2. * cov_a * dt;
+    Q.bottomRightCorner<3, 3>() = 1. / 4. * cov_a * dt * dt;
+    Q *= dt * dt;
+
+    return Q;
+}
+
+void SE3velCov::compound_2nd_order(const SE3vel &pose_increment, const Mat9 &Q, const double dt)
+{
+    Mat9 F(Mat9::Identity());
+    F.block<3, 3>(6, 3) = dt * Mat3::Identity();
+
+    Mat9 Gamma_inv_adj = pose_increment.inv().adj();
+
+    Mat9 tmp = Gamma_inv_adj * F;
+
+    this->covariance = tmp * this->covariance * tmp.transpose() + Q;
+
+    this->T_ = this->T() * pose_increment.T();
+}
+
+void SE3velCov::compound_4th_order(const SE3vel &pose_increment, const Mat9 &Q, const double dt)
+{
+    Mat9 S_4th = this->get_S_4th(Q); // get_s_4th mus be called exactly before calling compound_2nd
+    compound_2nd_order(pose_increment, Q, dt);
+
+    this->covariance += S_4th;
+}
+
+Mat9 SE3velCov::get_S_4th(const Mat9 &Q) const
+{
+    Mat9 S_4th(Mat9::Zero( ));
+
+    Mat9 A_sigma(Mat9::Zero());
+
+    Mat9 sigma = this->covariance;
+    Mat3 sigma_phi_phi = sigma.topLeftCorner<3, 3>();
+    Mat3 sigma_phi_nu = sigma.block<3, 3>(0, 3);
+    Mat3 sigma_nu_phi = sigma_phi_nu.transpose();
+    Mat3 sigma_phi_rho = sigma.topRightCorner<3, 3>();
+    Mat3 sigma_rho_phi = sigma_phi_rho.transpose();
+    Mat3 sigma_nu_nu = sigma.block<3, 3>(3, 3);
+    Mat3 sigma_nu_rho = sigma.block<3, 3>(3, 6);
+    Mat3 sigma_rho_rho = sigma.bottomRightCorner<3, 3>();
+
+    A_sigma.topLeftCorner<3, 3>() = brackets(sigma_phi_phi);
+    A_sigma.block<3, 3>(3, 0) = brackets(sigma_phi_nu.transpose() + sigma_phi_nu);
+    A_sigma.block<3, 3>(3, 3) = brackets(sigma_phi_phi);
+    A_sigma.bottomLeftCorner<3, 3>() = brackets(sigma_phi_rho.transpose() + sigma_phi_rho);
+    A_sigma.bottomRightCorner<3, 3>() = brackets(sigma_phi_phi);
+
+    Mat9 A_q(Mat9::Zero());
+
+    Mat3 q_phi_phi = Q.topLeftCorner<3, 3>();
+    Mat3 q_phi_nu = Q.block<3, 3>(0, 3);
+    Mat3 q_nu_phi = q_phi_nu.transpose();
+    Mat3 q_phi_rho = Q.topRightCorner<3, 3>();
+    Mat3 q_rho_phi = q_phi_rho.transpose();
+    Mat3 q_nu_nu = Q.block<3, 3>(3, 3);
+    Mat3 q_nu_rho = Q.block<3, 3>(3, 6);
+    Mat3 q_rho_rho = Q.bottomRightCorner<3, 3>();
+
+    A_q.topLeftCorner<3, 3>() = brackets(q_phi_phi);
+    A_q.block<3, 3>(3, 0) = brackets(q_phi_nu + q_phi_nu.transpose());
+    A_q.block<3, 3>(3, 3) = brackets(q_phi_phi);
+    A_q.bottomLeftCorner<3, 3>() = brackets(q_phi_rho + q_phi_rho.transpose());
+    A_q.bottomRightCorner<3, 3>() = brackets(q_phi_phi);
+
+    Mat9 B(Mat9::Zero());
+
+    B.topLeftCorner<3, 3>() = brackets(sigma_phi_phi, q_phi_phi);
+    B.block<3, 3>(3, 0) = brackets(sigma_phi_phi, q_phi_nu) + brackets(sigma_nu_phi, q_phi_phi);
+    B.block<3, 3>(0, 3) = B.block<3, 3>(3, 0).transpose();
+    B.block<3, 3>(6, 0) = brackets(sigma_phi_phi, q_phi_rho) + brackets(sigma_rho_phi, q_phi_phi);
+    B.block<3, 3>(0, 6) = B.block<3, 3>(6, 0).transpose();
+    B.block<3,3>(3,3) = brackets(sigma_phi_phi,q_nu_nu) + brackets(sigma_phi_nu,q_nu_phi) +
+                        brackets(sigma_nu_phi, q_nu_phi) + brackets(sigma_nu_nu,q_phi_phi);
+
+    B.block<3,3>(3,6) = brackets(sigma_nu_rho,q_phi_phi) + brackets(sigma_phi_rho,q_nu_phi) + 
+                        brackets(sigma_nu_phi,q_rho_phi) + brackets(sigma_phi_phi, q_nu_rho);
+    B.block<3,3>(6,3) = B.block<3,3>(3,6).transpose();
+    B.bottomRightCorner<3, 3>() = brackets(sigma_phi_phi,q_rho_rho) + brackets(sigma_phi_rho,q_rho_phi) + 
+                        brackets(sigma_rho_phi,q_phi_rho) + brackets(sigma_rho_rho,q_phi_phi);
+
+    S_4th = 1./12.*(A_sigma*Q + Q*A_sigma.transpose() + A_q*sigma + sigma*A_q.transpose()) + 1./4.*B;
+
+    return S_4th;
+}
diff --git a/src/geometry/SO3.cpp b/src/geometry/SO3.cpp
index 6ecbea2..bc49019 100644
--- a/src/geometry/SO3.cpp
+++ b/src/geometry/SO3.cpp
@@ -133,7 +133,7 @@ void SO3::exp(const Mat3 &w_hat)
     R_ << Mat3::Identity() + c1 * w_hat + c2 * w_hat *w_hat;
 }
 
-Mat3 SO3::ln(double *ro) const
+Mat3 SO3::ln() const
 {
     // Logarithmic mapping of the rotations
     Mat3 res;
@@ -236,7 +236,6 @@ Mat3 SO3::ln(double *ro) const
             // Note that o is the absolute value of the angle of rotation (don't flip)
         }
     }
-    if (ro != nullptr) *ro = o;
     return res;
 }
 
diff --git a/src/geometry/mrob/SE3.hpp b/src/geometry/mrob/SE3.hpp
index 60a5ecf..7533601 100644
--- a/src/geometry/mrob/SE3.hpp
+++ b/src/geometry/mrob/SE3.hpp
@@ -40,8 +40,8 @@
  *             [0  1]
  *  Associated to the groups of RBT, there is the Lie algebra se3
  *  representing the same transformation in the tangent space around the identity.
- *  Particularly, xi =[w , v] \in Re^6, where w \in Re^3 represents the rotation
- *  and v the translation.
+ *  Particularly, xi =[theta , pho] \in Re^6, where theta \in Re^3 represents the rotation
+ *  and pho the translation.
  *  We will preserve this order in this class.
  */
 namespace mrob{
@@ -218,6 +218,10 @@ Mat61 vee6(const Mat4 &xi_hat);
 
 bool isSE3(const Mat4 &T);
 
+
+Mat3 left_jacobian(const Mat31 &phi);
+Mat3 inv_left_jacobian(const Mat31 &phi);
+
 /**
  * Returns the generative matrix given the coordinate,
  * considering xi(0..5) = [theta(0..2), rho(3..5)]
diff --git a/src/geometry/mrob/SE3cov.hpp b/src/geometry/mrob/SE3cov.hpp
index 2f1677d..a210a89 100644
--- a/src/geometry/mrob/SE3cov.hpp
+++ b/src/geometry/mrob/SE3cov.hpp
@@ -143,6 +143,8 @@ namespace mrob
      * @return Mat6
      */
     Mat6 curly_wedge(const Mat61& xi);
+    Mat3 brackets(const Mat3 &A);
+    Mat3 brackets(const Mat3 &A, const Mat3 &B);
 
 } // end namespace
 
diff --git a/src/geometry/mrob/SE3vel.hpp b/src/geometry/mrob/SE3vel.hpp
new file mode 100644
index 0000000..cd624df
--- /dev/null
+++ b/src/geometry/mrob/SE3vel.hpp
@@ -0,0 +1,93 @@
+/* Copyright (c) 2022, Gonzalo Ferrer
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ *
+ * SE3vel.hpp
+ *
+ *  Created on: June 12, 2021
+ *      Author: Aleksei Panchenko
+ *              Gonzalo Ferrer
+ *              g.ferrer@skoltech.ru
+ *              Mobile Robotics Lab, Skoltech
+ */
+
+#ifndef SE3VEL_HPP_
+#define SE3VEL_HPP_
+
+
+#include "mrob/matrix_base.hpp"
+#include "mrob/SO3.hpp"
+
+namespace mrob{
+
+/**
+ *  \brief Extended Special Euclidean (group) in 3d
+ *  Is the group representing rotations, vel and translations:
+ *  SE3vel = {T = [R  t  v]  |  R \in SO3 , t \in Re^3, v \in Re^3 }
+ *                [0   I  ]
+ *  The Lie Algebra associated to this group is expressed by the coordinates
+ *      xi =[theta , pho, vel] \in Re^9, where theta \in Re^3 represents the rotation
+ *  and pho the translation and vel the velocity.
+ *  We will preserve this order in this class.
+ */
+
+class SE3vel{
+    public:
+        SE3vel(const Mat5 &T = Mat5::Identity());
+
+        SE3vel(const SE3vel &T);
+
+        SE3vel(const SO3 &R, const Mat31 &t, const Mat31 &v);
+
+        SE3vel(const Mat91 &xi);
+
+        SE3vel inv(void) const;
+
+        Mat31 t() const;
+        Mat31 v() const;
+        Mat3 R() const;
+        Mat5 T() const;
+        Mat<3,5> T_compact() const;
+
+
+        Mat9 adj() const;
+
+        void Exp(const Mat91 &xi);
+        Mat91 Ln(void) const;
+
+        void regenerate();
+        SE3vel operator*(const mrob::SE3vel& rhs);
+        SE3vel& operator=(const SE3vel& rhs);
+        std::ostream& operator<<(std::ostream &os);
+        /**
+         * @brief Generates string representation of the object
+         *
+         * @return std::string object to print
+         */
+        std::string toString() const;
+
+        void print() const;
+     
+    protected:
+        Mat5 T_;
+};
+
+Mat5 hat9(const Mat91 &xi);
+
+Mat91 vee9(const Mat5 &xi_hat);
+
+}// end namespace
+
+
+#endif /* SE3VEL_HPP_ */
diff --git a/src/geometry/mrob/SE3velCov.hpp b/src/geometry/mrob/SE3velCov.hpp
new file mode 100644
index 0000000..fb390c4
--- /dev/null
+++ b/src/geometry/mrob/SE3velCov.hpp
@@ -0,0 +1,60 @@
+/* Copyright (c) 2022, Gonzalo Ferrer
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ *
+ * SE3velCov.hpp
+ *
+ *  Created on: June 12, 2021
+ *      Author: Aleksei Panchenko
+ *              Gonzalo Ferrer
+ *              g.ferrer@skoltech.ru
+ *              Mobile Robotics Lab, Skoltech
+ */
+
+#ifndef SE3VELCOV_HPP_
+#define SE3VELCOV_HPP_
+
+#include "mrob/SE3vel.hpp"
+#include "mrob/SE3cov.hpp"
+
+namespace mrob
+{
+    class SE3velCov : public mrob::SE3vel
+    {
+    // TODO: this class needs deep review, since the parent class has been modified, on reversing the order of the 
+    // Lie coodinates to be [theta,t,v]. This affected:
+    //  - Contructor
+    //  - Ln, Exp
+    //  - Adjoint
+    public:
+        SE3velCov(void);
+        SE3velCov(const SE3vel &pose, const Mat9 &covariance);
+        Mat9 covariance;
+        void compound_2nd_order(const SE3vel &pose_increment, const Mat9 &increment_covariance, const double dt);
+        void compound_4th_order(const SE3vel &pose_increment, const Mat9 &increment_covariance, const double dt);
+
+        Mat9 getQ(const Mat3& cov_a, const Mat3& cov_w, const double dt) const;
+
+        Mat9 get_S_4th(const Mat9 &Q) const;
+
+        void print();
+
+        // transforms covariance matrix to notation from Barfoot's papers
+        // transform(transform(cov)) = cov - self-inverse
+        Mat9 transform(const Mat9 &covariance) const;
+    };
+
+} // end namespace
+
+#endif //SE3VELCOV_HPP_
diff --git a/src/geometry/mrob/SO3.hpp b/src/geometry/mrob/SO3.hpp
index ebe5690..d7979e8 100644
--- a/src/geometry/mrob/SO3.hpp
+++ b/src/geometry/mrob/SO3.hpp
@@ -110,7 +110,7 @@ class SO3
      *
      * Special cases for 0 and +-pi
      */
-    Mat3 ln(double *o = nullptr) const;
+    Mat3 ln() const;
     /**
      * Logarithm of R and then the vee operator to get the coordinates w of the Lie Algebra
      */
diff --git a/src/pybind/SE3py.cpp b/src/pybind/SE3py.cpp
index 1b10441..be86e03 100644
--- a/src/pybind/SE3py.cpp
+++ b/src/pybind/SE3py.cpp
@@ -29,6 +29,7 @@
 #include "mrob/SE3.hpp"
 #include "mrob/SO3.hpp"
 #include "mrob/SE3cov.hpp"
+#include "mrob/SE3vel.hpp"
 #include <pybind11/eigen.h>
 #include <pybind11/pybind11.h>
 //#include <pybind11/stl.h>
@@ -101,7 +102,7 @@ void init_geometry(py::module &m) {
         .def("__mul__", &SE3::operator*, py::is_operator())
         .def("__str__", &SE3::toString, "Generates string representation of the SE3 object for print() output")
         .def("__repr__", &SE3::toString, "Generates string representation of the SE3 object for console output");
-;
+
     m.def("isSE3", &mrob::isSE3, "Returns True is the matrix is a valid transformation and False if not");
 
     py::class_<SO3>(m, "SO3")
@@ -136,7 +137,7 @@ void init_geometry(py::module &m) {
         .def("__mul__", &SO3::operator*, py::is_operator())
         .def("__str__", &SO3::toString, "Generates string representation of the SO3 object for print() output")
         .def("__repr__", &SO3::toString, "Generates string representation of the SO3 object for console output");
-        ;
+
     m.def("isSO3", &mrob::isSO3, "Returns True is the matrix is a valid rotation and False if not");
 
     m.def("hat3", &mrob::hat3, "Returns a skew symmetric matrix 3x3 from a 3-vector", py::return_value_policy::copy);
@@ -148,40 +149,83 @@ void init_geometry(py::module &m) {
     m.def("rpy_to_so3",  &rpy_to_so3,"Suport function from roll pitch yaw to a rotation");
 
     py::class_<SE3Cov, SE3>(m, "SE3Cov")
-            .def(py::init<>(),
-                 "Default construct a new SE3Cov object",
-                 py::return_value_policy::copy)
-            .def(py::init<const SE3 &, const Mat6 &>(),
-                 "Construtor of SE3Cov object with given pose and covariance",
-                 py::return_value_policy::copy)
-            .def(py::init<const SE3Cov &>(),
-                 "Copy constructor",
-                 py::return_value_policy::copy)
-            .def("cov", &SE3Cov::cov,
-                 "returns current covariance matrix state",
-                 py::return_value_policy::copy)
-            .def("compound_2nd_order",
-                 py::overload_cast<const SE3Cov &>(&SE3Cov::compound_2nd_order, py::const_),
-                 "Pose uncertainty compounding of the second order.")
-            .def("compound_2nd_order",
-                 py::overload_cast<const SE3 &, const Mat6 &>(&SE3Cov::compound_2nd_order, py::const_),
-                 "Pose uncertainty compounding of the second order.")
-            .def("compound_4th_order",
-                 py::overload_cast<const SE3Cov &>(&SE3Cov::compound_4th_order, py::const_),
-                 "SE3pose uncertainy compounding of the fourth order.")
-            .def("compound_4th_order",
-                 py::overload_cast<const SE3 &, const Mat6 &>(&SE3Cov::compound_4th_order, py::const_),
-                 "SE3pose uncertainy compounding of the fourth order.")
-            .def("print",
-                 &SE3Cov::print,
-                 "Prints current state of pose and covariance.")
-            .def("mul",
-                 py::overload_cast<const SE3Cov &>(&SE3Cov::mul, py::const_),
-                 "Multiplication method mul() as an interface for compounding.",
-                 py::return_value_policy::copy)
-            .def("__mul__", &SE3Cov::operator*, py::is_operator())
-            .def("__str__", &SE3Cov::toString, "Generates string representation of the SE3cov object for print() output")
-            .def("__repr__", &SE3Cov::toString, "Generates string representation of the SE3cov object for console output");
-        m.def("curley_wedge", &mrob::curly_wedge, "Returns 6-by-6 matrix, the output of curley wedge operator.", py::return_value_policy::copy);
+        .def(py::init<>(),
+                "Default construct a new SE3Cov object",
+                py::return_value_policy::copy)
+        .def(py::init<const SE3 &, const Mat6 &>(),
+                "Construtor of SE3Cov object with given pose and covariance",
+                py::return_value_policy::copy)
+        .def(py::init<const SE3Cov &>(),
+                "Copy constructor",
+                py::return_value_policy::copy)
+        .def("cov", &SE3Cov::cov,
+                "returns current covariance matrix state",
+                py::return_value_policy::copy)
+        .def("compound_2nd_order",
+                py::overload_cast<const SE3Cov &>(&SE3Cov::compound_2nd_order, py::const_),
+                "Pose uncertainty compounding of the second order.")
+        .def("compound_2nd_order",
+                py::overload_cast<const SE3 &, const Mat6 &>(&SE3Cov::compound_2nd_order, py::const_),
+                "Pose uncertainty compounding of the second order.")
+        .def("compound_4th_order",
+                py::overload_cast<const SE3Cov &>(&SE3Cov::compound_4th_order, py::const_),
+                "SE3pose uncertainy compounding of the fourth order.")
+        .def("compound_4th_order",
+                py::overload_cast<const SE3 &, const Mat6 &>(&SE3Cov::compound_4th_order, py::const_),
+                "SE3pose uncertainy compounding of the fourth order.")
+        .def("print",
+                &SE3Cov::print,
+                "Prints current state of pose and covariance.")
+        .def("mul",
+                py::overload_cast<const SE3Cov &>(&SE3Cov::mul, py::const_),
+                "Multiplication method mul() as an interface for compounding.",
+                py::return_value_policy::copy)
+        .def("__mul__", &SE3Cov::operator*, py::is_operator())
+        .def("__str__", &SE3Cov::toString, "Generates string representation of the SE3cov object for print() output")
+        .def("__repr__", &SE3Cov::toString, "Generates string representation of the SE3cov object for console output");
+    m.def("curley_wedge", &mrob::curly_wedge, "Returns 6-by-6 matrix, the output of curley wedge operator.", py::return_value_policy::copy);
+
+    py::class_<SE3vel>(m, "SE3vel")
+        .def(py::init<>(),
+                "Default construct a new SE3vel object",
+                py::return_value_policy::copy)
+        .def(py::init<const SO3 &, const Mat31 &, const Mat31 &>(),
+                "Construtor of SE3vel object with given Rotation, translation and velocity",
+                py::return_value_policy::copy)
+        .def(py::init<const Mat91 &>(),
+        "Given a vector xi in R^9, creates a tranformation with the exponential mapping. Order is [rot, tran, vel]",
+        py::return_value_policy::copy)
+        .def(py::init<const SE3vel &>(),
+                "Copy constructor",
+                py::return_value_policy::copy)
+        .def("t", &SE3vel::t,
+                "returns current covariance matrix state",
+                py::return_value_policy::copy)
+        .def("T", &SE3vel::T,
+                "Outputs a 4x4 array with the transformation",
+                py::return_value_policy::copy)
+        .def("R",&SE3vel::R,
+                "Outputs the Rotation array 3x3 component of the transformation",
+                py::return_value_policy::copy)
+        .def("v", &SE3vel::v,
+                "Outputs the velocity array 3D component of the transformation",
+                py::return_value_policy::copy)
+        .def("T_compact", &SE3vel::T_compact,
+                "Outputs the 3x5 (compact) transformation [R,t,v]",
+                py::return_value_policy::copy)
+        .def("Ln", &SE3vel::Ln,
+                "Logarithm + vee operator, returns 9D coordinates of the tangent space around the identity",
+                py::return_value_policy::copy)
+        .def("adj", &SE3vel::adj,
+                "Outputs the adjoint of T a 9x9 matrix",
+                py::return_value_policy::copy)
+        .def("inv", &SE3vel::inv,
+                "Outputs the inverse of T",
+                py::return_value_policy::copy)
+        .def("print", &SE3vel::print,
+                "Prints current state of pose.")
+        .def("__mul__", &SE3vel::operator*, py::is_operator())
+        .def("__str__", &SE3vel::toString, "Generates string representation of the SE3vel object for print() output")
+        .def("__repr__", &SE3vel::toString, "Generates string representation of the SE3vel object for console output");
 }
 


Test patch:
diff --git a/mrobpy/tests/se3_vel_test.py b/mrobpy/tests/se3_vel_test.py
new file mode 100644
index 0000000..6be468b
--- /dev/null
+++ b/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)
+
+