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

Livox_automatic_calibration-master/include/geometry_utils/GeometryUtils.h

@@ -0,0 +1,138 @@
+/*
+  geometry_utils: Utility library to provide common geometry types and transformations
+  Copyright (C) 2014  Nathan Michael
+                2016  Erik Nelson
+
+  This program is free software; you can redistribute it and/or
+  modify it under the terms of the GNU General Public License
+  as published by the Free Software Foundation; either version 2
+  of the License, or (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+*/
+
+#ifndef GEOMETRY_UTILS_H
+#define GEOMETRY_UTILS_H
+
+#include "Vector2.h"
+#include "Vector3.h"
+#include "Vector4.h"
+#include "Quaternion.h"
+#include "Matrix2x2.h"
+#include "Matrix3x3.h"
+#include "Matrix4x4.h"
+#include "Rotation2.h"
+#include "Rotation3.h"
+#include "Transform2.h"
+#include "Transform3.h"
+
+namespace geometry_utils {
+inline double Unroll(double x) {
+  x = fmod(x, 2.0 * M_PI);
+  if (x < 0)
+    x += 2.0 * M_PI;
+  return x;
+}
+
+inline double Normalize(double x) {
+  x = fmod(x + M_PI, 2.0 * M_PI);
+  if (x < 0)
+    x += 2.0 * M_PI;
+  return x - M_PI;
+}
+
+inline double S1Distance(double from, double to) {
+  double result = Unroll(Unroll(to) - Unroll(from));
+  if (result > M_PI)
+    result = -(2.0 * M_PI - result);
+  return Normalize(result);
+}
+
+inline double Rad2Deg(double angle) {
+  return angle * 180.0 * M_1_PI;
+}
+
+inline double Deg2Rad(double angle) {
+  return angle * M_PI / 180.0;
+}
+
+inline Vec3 Rad2Deg(const Vec3& angles) {
+  return Vec3(Rad2Deg(angles(0)), Rad2Deg(angles(1)), Rad2Deg(angles(2)));
+}
+
+inline Vec3 Deg2Rad(const Vec3& angles) {
+  return Vec3(Deg2Rad(angles(0)), Deg2Rad(angles(1)), Deg2Rad(angles(2)));
+}
+
+inline Vec3 RToZYX(const Rot3& rot) {
+  return rot.GetEulerZYX();
+}
+
+inline Rot3 ZYXToR(const Vec3& angles) {
+  return Rot3(angles);
+}
+
+inline Rot3 QuatToR(const Quat& quat) {
+  return Rot3(quat);
+}
+
+inline Quat RToQuat(const Rot3& rot) {
+  return Quat(Eigen::Quaterniond(rot.Eigen()));
+}
+
+inline double GetRoll(const Rot3& r) {
+  return r.Roll();
+}
+
+inline double GetRoll(const Quat& q) {
+  return Rot3(q).Roll();
+}
+
+inline double GetPitch(const Rot3& r) {
+  return r.Pitch();
+}
+
+inline double GetPitch(const Quat& q) {
+  return Rot3(q).Pitch();
+}
+
+inline double GetYaw(const Rot3& r) {
+  return r.Yaw();
+}
+
+inline double GetYaw(const Quat& q) {
+  return Rot3(q).Yaw();
+}
+
+inline double SO3Error(const Quat& q1, const Quat& q2) {
+  return Rot3(q1).Error(Rot3(q2));
+}
+
+inline double SO3Error(const Rot3& r1, const Rot3& r2) {
+  return r1.Error(r2);
+}
+
+inline Vec3 CartesianToSpherical(const Vec3& v) {
+  double rho = v.Norm();
+  return Vec3(rho, acos(v.Z() / rho), atan2(v.Y(), v.X()));
+}
+
+inline Vec3 SphericalToCartesian(const Vec3& v) {
+  return Vec3(v(0) * sin(v(1)) * cos(v(2)), v(0) * sin(v(1)) * sin(v(2)),
+              v(0) * cos(v(1)));
+}
+
+inline Vec3 NEDCartesian(const Vec3& v) {
+  return Vec3(v(0), -v(1), -v(2));
+}
+
+}
+
+#endif