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

Livox_automatic_calibration-master/include/geometry_utils/Transform2.h

@@ -0,0 +1,108 @@
+#ifndef GEOMETRY_UTILS_TRANSFORM2_H
+#define GEOMETRY_UTILS_TRANSFORM2_H
+
+#include "Vector2.h"
+#include "Rotation2.h"
+
+namespace geometry_utils {
+
+template <typename T>
+struct Transform2Base {
+  typedef boost::shared_ptr<Transform2Base> Ptr;
+  typedef boost::shared_ptr<const Transform2Base> ConstPtr;
+
+  Vector2Base<T> translation;
+  Rotation2Base<T> rotation;
+
+  Transform2Base() {
+    translation.Zeros();
+    rotation.Eye();
+  }
+
+  Transform2Base(const Vector2Base<T>& translation_,
+                 const Rotation2Base<T>& rotation_)
+      : translation(translation_), rotation(rotation_) {}
+
+  Transform2Base(const Transform2Base<T>& in)
+      : translation(in.translation), rotation(in.rotation) {}
+
+  Transform2Base(T x, T y, T th) : translation(x, y), rotation(th) {}
+
+  Transform2Base& operator=(const Transform2Base& rhs) {
+    if (this == &rhs)
+      return *this;
+    translation = rhs.translation;
+    rotation = rhs.rotation;
+    return *this;
+  }
+
+  Vector2Base<T> operator*(const Vector2Base<T>& p) const {
+    return rotation * p + translation;
+  }
+
+  Transform2Base<T> operator+(const Transform2Base<T>& t) const {
+    return Transform2Base<T>(translation + rotation * t.translation,
+                             rotation * t.rotation);
+  }
+
+  bool operator==(const Transform2Base& that) const {
+    return this->Equals(that);
+  }
+
+  bool operator!=(const Transform2Base& that) const {
+    return !this->Equals(that);
+  }
+
+  bool Equals(const Transform2Base& that, const T ptol = 1e-5,
+              const T rtol = 1e-5) const {
+    return (translation.Equals(that.translation, ptol) &&
+            rotation.Equals(that.rotation, rtol));
+  }
+
+  void Print(const std::string& prefix = std::string()) const {
+    if (!prefix.empty())
+      std::cout << prefix << std::endl;
+    std::cout << (*this) << std::endl;
+  }
+
+  static Transform2Base Identity() {
+    return Transform2Base();
+  }
+};
+
+template <typename T>
+std::ostream& operator<<(std::ostream& out, const Transform2Base<T>& m) {
+  out << "Translation:" << std::endl << m.translation << std::endl;
+  out << "Rotation:" << std::endl << m.rotation;
+  return out;
+}
+
+template <typename T>
+Transform2Base<T> PoseUpdate(const Transform2Base<T>& t1,
+                             const Transform2Base<T>& t2) {
+  return Transform2Base<T>(t1.translation + t1.rotation * t2.translation,
+                           t1.rotation * t2.rotation);
+}
+
+template <typename T>
+Transform2Base<T> PoseInverse(const Transform2Base<T>& t) {
+  return Transform2Base<T>(-1.0 * t.rotation.Trans() * t.translation,
+                           t.rotation.Trans());
+}
+
+template <typename T>
+Transform2Base<T> PoseDelta(const Transform2Base<T>& t1,
+                            const Transform2Base<T>& t2) {
+  return Transform2Base<T>(
+      t1.rotation.Trans() * (t2.translation - t1.translation),
+      t1.rotation.Trans() * t2.rotation);
+}
+
+typedef Transform2Base<float> Transform2f;
+typedef Transform2Base<double> Transform2d;
+typedef Transform2d Transform2;
+typedef Transform2 Tr2;
+
+}
+
+#endif