feat: 初始化 Go2 导航代码与配置

Livox_automatic_calibration-master/include/geometry_utils/Matrix2x2.h

@@ -0,0 +1,97 @@
+#ifndef GEOMETRY_UTILS_MATRIX2X2_H
+#define GEOMETRY_UTILS_MATRIX2X2_H
+
+#include "MatrixNxNBase.h"
+#include "Vector2.h"
+
+namespace geometry_utils {
+
+template <typename T>
+struct Matrix2x2Base : MatrixNxNBase<T, 2> {
+  Matrix2x2Base() : MatrixNxNBase<T, 2>() {}
+  Matrix2x2Base(T val) : MatrixNxNBase<T, 2>(val) {}
+  Matrix2x2Base(const Matrix2x2Base& in) : MatrixNxNBase<T, 2>(in.data) {}
+  Matrix2x2Base(const boost::array<T, 4>& in) : MatrixNxNBase<T, 2>(in) {}
+  Matrix2x2Base(T (&in)[2 * 2]) : MatrixNxNBase<T, 2>(in) {}
+  Matrix2x2Base(const Eigen::Matrix<T, 2, 2>& in) : MatrixNxNBase<T, 2>(in) {}
+  Matrix2x2Base(const MatrixNxNBase<T, 2>& in) : MatrixNxNBase<T, 2>(in) {}
+  Matrix2x2Base(const MatrixNxMBase<T, 2, 2>& in) : MatrixNxNBase<T, 2>(in) {}
+
+  Matrix2x2Base(T R11, T R12, T R21, T R22) {
+    this->data[0] = R11;
+    this->data[1] = R12;
+    this->data[2] = R21;
+    this->data[3] = R22;
+  }
+
+  virtual inline T Det() const {
+    T a = this->data[0];
+    T b = this->data[1];
+    T c = this->data[2];
+    T d = this->data[3];
+    return (-(b * c) + a * d);
+  }
+
+  virtual inline MatrixNxNBase<T, 2> Inv() const {
+    Vector2Base<T> e(SingularValues());
+
+    T emax = e(0);
+    T emin = e(1);
+
+    if (emin < std::numeric_limits<T>::denorm_min())
+      throw std::runtime_error("Matrix2x2Base: appears singular");
+
+    if (emax / emin > std::numeric_limits<T>::epsilon()) {
+      T a = this->data[0];
+      T b = this->data[1];
+      T c = this->data[2];
+      T d = this->data[3];
+
+      T tmp[4] = {d / (-b * c + a * d), b / (b * c - a * d),
+                  c / (b * c - a * d),  a / (-b * c + a * d)};
+      return MatrixNxNBase<T, 2>(tmp);
+    } else
+      throw std::runtime_error("Matrix2x2Base: appears singular");
+  }
+
+  virtual inline Vector2Base<T> SingularValues() const {
+    T a = this->data[0];
+    T b = this->data[1];
+    T c = this->data[2];
+    T d = this->data[3];
+
+    T tmp1 = a * a + b * b + c * c + d * d;
+    T tmp2 = math::sqrt((math::pow(b + c, static_cast<T>(2)) +
+                         math::pow(a - d, static_cast<T>(2))) *
+                        (math::pow(b - c, static_cast<T>(2)) +
+                         math::pow(a + d, static_cast<T>(2))));
+
+	T e1 = 0.0;// math::sqrt(tmp1 - tmp2) * M_SQRT1_2;
+	T e2 = 0.0; //math::sqrt(tmp1 + tmp2) * M_SQRT1_2;
+
+    return Vector2Base<T>(e1 > e2 ? e1 : e2, e1 < e2 ? e1 : e2);
+  }
+};
+
+inline Matrix2x2Base<float> operator*(const float& lhs,
+                                      const Matrix2x2Base<float>& rhs) {
+  return Matrix2x2Base<float>(rhs * lhs);
+}
+
+inline Matrix2x2Base<double> operator*(const double& lhs,
+                                       const Matrix2x2Base<double>& rhs) {
+  return Matrix2x2Base<double>(rhs * lhs);
+}
+
+typedef Matrix2x2Base<float> Matrix2x2f;
+typedef Matrix2x2Base<float> Mat22f;
+
+typedef Matrix2x2Base<double> Matrix2x2d;
+typedef Matrix2x2Base<double> Mat22d;
+
+typedef Matrix2x2Base<double> Matrix2x2;
+typedef Matrix2x2Base<double> Mat22;
+
+}
+
+#endif