feat:added math_operatiosn.cpp and .hpp for the math commands support…

…ing new eigen library in ros2

navigation/ESKF2/CMakeLists.txt

@@ -7,9 +7,10 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

navigation/ESKF2/package.xml

@@ -8,6 +8,16 @@
   <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>rclcpp</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
+  <build_depend>tf2_sensor_msgs</build_depend>
+  <build_depend>tf2_eigen</build_depend>
+  <build_depend>eigen3_cmake_module</build_depend>
+  <exec_depend>rclcpp</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

navigation/ESKF2/src/main.cpp

@@ -1,3 +1,12 @@
-#include "main.hpp"
+#include "../include/ESKF2/main.hpp"
 
 
+int main(int argc, char **argv)
+{
+    ros2::init(argc, argv, "ESKF2");
+    ros2::NodeHandle nh;
+    ESKF2 eskf2(nh);
+    ros::spin();
+    return 0;
+}
+

navigation/ESKF2/src/math_operations.cpp

@@ -0,0 +1,201 @@
+#include "rclcpp/rclcpp.hpp"
+#include "eigen3/Eigen/Dense"
+#include "geometry_msgs/msg/quaternion.hpp"
+#include <vector>
+#include <cmath>
+#include <algorithm>
+
+using Eigen::Matrix3d;
+using Eigen::MatrixXd;
+using Eigen::Vector3d;
+using Eigen::Vector4d;
+using Eigen::VectorXd;
+
+// Convert ROS Quaternion to Eigen Vector4d
+Vector4d quaternionToEigen(const geometry_msgs::msg::Quaternion& quat)
+{
+  return Vector4d(quat.w, quat.x, quat.y, quat.z);
+}
+
+// Convert Eigen Vector4d to ROS Quaternion
+geometry_msgs::msg::Quaternion eigenToQuaternion(const Vector4d& quatEigen)
+{
+  geometry_msgs::msg::Quaternion quat;
+  quat.w = quatEigen(0);
+  quat.x = quatEigen(1);
+  quat.y = quatEigen(2);
+  quat.z = quatEigen(3);
+  return quat;
+}
+
+// Cross-product matrix
+Matrix3d crossProductMatrix(const Vector3d& n)
+{
+  Matrix3d skewMatrix = Matrix3d::Zero();
+  skewMatrix << 0, -n(2), n(1),
+                n(2), 0, -n(0),
+                -n(1), n(0), 0;
+  return skewMatrix;
+}
+
+// Quaternion Hamilton Product
+Vector4d quaternionHamiltonProduct(VectorXd quatLeft, VectorXd quatRight)
+{
+  Vector4d quatProduct = Vector4d::Zero();
+  Vector3d imaginaryPartOfLeftQuaternion;
+  Vector3d imaginaryPartOfRightQuaternion;
+
+  if (quatLeft.size() == 3)  // Assume pure quaternion
+  {
+    quatLeft.conservativeResize(4);
+    quatLeft << 0, quatLeft(0), quatLeft(1), quatLeft(2);
+  }
+  if (quatRight.size() == 3)  // Assume pure quaternion
+  {
+    quatRight.conservativeResize(4);
+    quatRight << 0, quatRight(0), quatRight(1), quatRight(2);
+  }
+
+  imaginaryPartOfLeftQuaternion = quatLeft.tail<3>();
+  imaginaryPartOfRightQuaternion = quatRight.tail<3>();
+
+  quatProduct << quatLeft(0) * quatRight(0) -
+                   imaginaryPartOfLeftQuaternion.transpose() * imaginaryPartOfRightQuaternion,
+                 quatRight(0) * imaginaryPartOfLeftQuaternion +
+                   quatLeft(0) * imaginaryPartOfRightQuaternion +
+                   crossProductMatrix(imaginaryPartOfLeftQuaternion) * imaginaryPartOfRightQuaternion;
+
+  return quatProduct;
+}
+
+// Build block diagonal matrix from four 3x3 matrices
+MatrixXd blk3x3Diag(const Matrix3d& matrixA, const Matrix3d& matrixB, const Matrix3d& matrixC, const Matrix3d& matrixD)
+{
+  int numberOfMatrices = 4;
+  MatrixXd bdm = MatrixXd::Zero(3 * numberOfMatrices, 3 * numberOfMatrices);
+
+  bdm.block(0, 0, 3, 3) = matrixA;
+  bdm.block(3, 3, 3, 3) = matrixB;
+  bdm.block(6, 6, 3, 3) = matrixC;
+  bdm.block(9, 9, 3, 3) = matrixD;
+
+  return bdm;
+}
+
+// Convert quaternion to rotation matrix
+Matrix3d quaternion2RotationMatrix(const Vector4d& quaternion)
+{
+  double w = quaternion(0);
+  Vector3d q = quaternion.tail<3>();
+  Matrix3d S = crossProductMatrix(q);
+  return Matrix3d::Identity() + 2 * w * S + 2 * S * S;
+}
+
+// Jacobian of DVL wrt quaternion
+MatrixXd jacobianFdOfDVL(const VectorXd& fun, const Vector4d& q, const double& step, const Vector3d& velWorld)
+{
+  Vector4d x = q;
+  Vector4d xPerturbed;
+  Vector3d fPerturbed;
+  MatrixXd jacobianMatrix(fun.rows(), 4);
+
+  for (int i = 0; i < 4; ++i)
+  {
+    xPerturbed = x;
+    xPerturbed(i) += step;
+    fPerturbed = quaternion2RotationMatrix(xPerturbed) * velWorld;
+    jacobianMatrix.col(i) = (fPerturbed - fun) / step;
+  }
+
+  return jacobianMatrix;
+}
+
+// Convert Euler angles to rotation matrix
+Matrix3d eulerToRotationMatrix(const Vector3d& eulerAngles)
+{
+  double roll = eulerAngles(0);
+  double pitch = eulerAngles(1);
+  double yaw = eulerAngles(2);
+
+  Matrix3d R_roll;
+  R_roll << 1, 0, 0,
+            0, cos(roll), -sin(roll),
+            0, sin(roll), cos(roll);
+
+  Matrix3d R_pitch;
+  R_pitch << cos(pitch), 0, sin(pitch),
+             0, 1, 0,
+             -sin(pitch), 0, cos(pitch);
+
+  Matrix3d R_yaw;
+  R_yaw << cos(yaw), -sin(yaw), 0,
+           sin(yaw), cos(yaw), 0,
+           0, 0, 1;
+
+  return R_yaw * R_pitch * R_roll;
+}
+
+// Convert Quaternion to Euler angles
+struct EulerAngles
+{
+  double roll, pitch, yaw;
+};
+
+EulerAngles fromQuaternionToEulerAngles(const geometry_msgs::msg::Quaternion& q)
+{
+  EulerAngles angles;
+  double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
+  double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
+  angles.roll = std::atan2(sinr_cosp, cosr_cosp);
+
+  double sinp = 2 * (q.w * q.y - q.z * q.x);
+  if (std::abs(sinp) >= 1)
+    angles.pitch = std::copysign(M_PI / 2, sinp);
+  else
+    angles.pitch = std::asin(sinp);
+
+  double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
+  double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
+  angles.yaw = std::atan2(siny_cosp, cosy_cosp);
+
+  return angles;
+}
+
+// Convert Euler angles to Quaternion
+geometry_msgs::msg::Quaternion fromRPYToQuaternion(const EulerAngles& angles)
+{
+  geometry_msgs::msg::Quaternion q;
+  double cy = cos(angles.yaw * 0.5);
+  double sy = sin(angles.yaw * 0.5);
+  double cp = cos(angles.pitch * 0.5);
+  double sp = sin(angles.pitch * 0.5);
+  double cr = cos(angles.roll * 0.5);
+  double sr = sin(angles.roll * 0.5);
+
+  q.w = cy * cp * cr + sy * sp * sr;
+  q.x = cy * cp * sr - sy * sp * cr;
+  q.y = sy * cp * sr + cy * sp * cr;
+  q.z = sy * cp * cr - cy * sp * sr;
+
+  return q;
+}
+
+// Simple vector statistics
+double meanOfVector(const std::vector<double>& vec)
+{
+  double sum = std::accumulate(vec.begin(), vec.end(), 0.0);
+  return sum / vec.size();
+}
+
+double maxOfVector(const std::vector<double>& vec)
+{
+  return *std::max_element(vec.begin(), vec.end());
+}
+
+double standardDeviationOfVector(const std::vector<double>& vec)
+{
+  double mean = meanOfVector(vec);
+  double sq_sum = std::inner_product(vec.begin(), vec.end(), vec.begin(), 0.0, std::plus<>(),
+                                     [mean](double a, double b) { return (a - mean) * (b - mean); });
+  return std::sqrt(sq_sum / (vec.size() - 1));
+}

navigation/ESKF2/src/math_operations.hpp

@@ -0,0 +1,31 @@
+#pragma once
+#include <eigen3/Eigen/Dense>
+#include <iostream>
+#include <cmath>
+#include <vector>
+
+constexpr long double PI{ 3.1415926535 };
+
+struct Quat
+{
+  double w, x, y, z;
+};
+
+struct EulerAngles
+{
+  double roll, pitch, yaw;
+};
+
+using namespace Eigen;
+
+MatrixXd blk3x3Diag(const Matrix3d& matrixA, const Matrix3d& matrixB, const Matrix3d& matrixC, const Matrix3d& matrixD);
+Matrix3d crossProductMatrix(const Vector3d& n);
+Vector4d quaternionHamiltonProduct(VectorXd quatLeft, VectorXd quatRight);
+Matrix3d quaternion2Rotationmatrix(const Vector4d& quaternion);
+MatrixXd jacobianFdOfDVL(const VectorXd& fun, const Quaterniond& q, const double& step, const Vector3d& velWorld);
+Matrix3d eulerToRotationMatrix(const Vector3d& eulerAngles);
+EulerAngles fromQuaternionToEulerAngles(const Quat& q);
+Quat fromRPYToQuaternion(const EulerAngles& angles);  // yaw (Z), pitch (Y), roll (X)
+double meanOfVector(const std::vector<double>& vec);
+double maxOfVector(const std::vector<double>& vec);
+double stanardDeviationOfVector(const std::vector<double>& vec);