lx/Go2_navigation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
龙正扬应用学生后端
2026-10-14 11:08:44 +08:00
parent 8e16ecceba
commit 9dab37e8c8
1258 changed files with 1936673 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

138
Livox_automatic_calibration-master/include/geometry_utils/GeometryUtils.h Executable file
View File

@@ -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

83
Livox_automatic_calibration-master/include/geometry_utils/GeometryUtilsMath.h Executable file
View File

@@ -0,0 +1,83 @@
/*
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_MATH_H
#define GEOMETRY_UTILS_MATH_H
#include <cmath>
namespace geometry_utils {
namespace math {
template<typename T> inline T cos(T in);
template<> inline float cos(float in) { return ::cosf(in); }
template<> inline double cos(double in) { return ::cos(in); }
template<typename T> inline T acos(T in);
template<> inline float acos(float in) { return ::acosf(in); }
template<> inline double acos(double in) { return ::acos(in); }
template<typename T> inline T sin(T in);
template<> inline float sin(float in) { return ::sinf(in); }
template<> inline double sin(double in) { return ::sin(in); }
template<typename T> inline T asin(T in);
template<> inline float asin(float in) { return ::asinf(in); }
template<> inline double asin(double in) { return ::asin(in); }
template<typename T> inline T tan(T in);
template<> inline float tan(float in) { return ::tanf(in); }
template<> inline double tan(double in) { return ::tan(in); }
template<typename T> inline T atan(T in);
template<> inline float atan(float in) { return ::atanf(in); }
template<> inline double atan(double in) { return ::atan(in); }
template<typename T> inline T fabs(T in);
template<> inline float fabs(float in) { return ::fabsf(in); }
template<> inline double fabs(double in) { return ::fabs(in); }
template<typename T> inline T fmin(T v1, T v2);
template<> inline float fmin(float v1, float v2) { return ::fminf(v1, v2); }
template<> inline double fmin(double v1, double v2) { return ::fmin(v1, v2); }
template<typename T> inline T fmax(T v1, T v2);
template<> inline float fmax(float v1, float v2) { return ::fmaxf(v1, v2); }
template<> inline double fmax(double v1, double v2) { return ::fmax(v1, v2); }
template<typename T> inline T sqrt(T in);
template<> inline float sqrt(float in) { return ::sqrtf(in); }
template<> inline double sqrt(double in) { return ::sqrt(in); }
template<typename T> inline T pow(T in, T exp);
template<> inline float pow(float in, float exp) { return ::powf(in, exp); }
template<> inline double pow(double in, double exp) { return ::pow(in, exp); }
template<typename T> inline T atan2(T y, T x);
template<> inline float atan2(float y, float x) { return ::atan2f(y, x); }
template<> inline double atan2(double y, double x) { return ::atan2(y, x); }
template<typename T> inline T hypot(T x, T y);
template<> inline float hypot(float x, float y) { return ::hypotf(x, y); }
template<> inline double hypot(double x, double y) { return ::hypot(x, y); }
}
}
#endif

73
Livox_automatic_calibration-master/include/geometry_utils/GeometryUtilsSerialization.h Executable file
View File

@@ -0,0 +1,73 @@
/*
geometry_utils: Utility library to provide common geometry types and transformations
Copyright (C) 2013 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_SERIALIZATION_H
#define GEOMETRY_UTILS_SERIALIZATION_H
#include "GeometryUtils.h"
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
namespace boost {
namespace serialization {
template<class Archive>
void Serialize(Archive & ar, geometry_utils::Vector2& v,
const unsigned int version) {
ar & v(0);
ar & v(1);
}
template<class Archive>
void Serialize(Archive & ar, geometry_utils::Vector3& v,
const unsigned int version) {
ar & v(0);
ar & v(1);
ar & v(2);
}
template<class Archive>
void Serialize(Archive & ar, geometry_utils::Vector4& v,
const unsigned int version) {
ar & v(0);
ar & v(1);
ar & v(2);
ar & v(3);
}
template<class Archive>
void Serialize(Archive & ar, geometry_utils::Matrix3x3& m,
const unsigned int version) {
for (unsigned int i = 0; i < 3; i++)
for (unsigned int j = 0; j < 3; j++)
ar & m(i, j);
}
template<class Archive>
void Serialize(Archive & ar, geometry_utils::Transform& t,
const unsigned int version) {
ar & t.translation;
ar & t.rotation;
}
}
}
#endif

97
Livox_automatic_calibration-master/include/geometry_utils/Matrix2x2.h Executable file
View File

@@ -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

101
Livox_automatic_calibration-master/include/geometry_utils/Matrix3x3.h Executable file
View File

@@ -0,0 +1,101 @@
#ifndef GEOMETRY_UTILS_MATRIX3X3_H
#define GEOMETRY_UTILS_MATRIX3X3_H
#include "MatrixNxNBase.h"
namespace geometry_utils {
template <typename T>
struct Matrix3x3Base : MatrixNxNBase<T, 3> {
Matrix3x3Base() : MatrixNxNBase<T, 3>() {}
Matrix3x3Base(T val) : MatrixNxNBase<T, 3>(val) {}
Matrix3x3Base(const Matrix3x3Base& in) : MatrixNxNBase<T, 3>(in.data) {}
Matrix3x3Base(const boost::array<T, 9>& in) : MatrixNxNBase<T, 3>(in) {}
Matrix3x3Base(T (&in)[9]) : MatrixNxNBase<T, 3>(in) {}
Matrix3x3Base(const Eigen::Matrix<T, 3, 3>& in) : MatrixNxNBase<T, 3>(in) {}
Matrix3x3Base(const MatrixNxNBase<T, 3>& in) : MatrixNxNBase<T, 3>(in) {}
Matrix3x3Base(const MatrixNxMBase<T, 3, 3>& in) : MatrixNxNBase<T, 3>(in) {}
Matrix3x3Base(T R11, T R12, T R13, T R21, T R22, T R23, T R31, T R32, T R33) {
this->data[0] = R11;
this->data[1] = R12;
this->data[2] = R13;
this->data[3] = R21;
this->data[4] = R22;
this->data[5] = R23;
this->data[6] = R31;
this->data[7] = R32;
this->data[8] = R33;
}
inline T Det() const {
T a = this->data[0];
T b = this->data[1];
T c = this->data[2];
T d = this->data[3];
T e = this->data[4];
T f = this->data[5];
T g = this->data[6];
T h = this->data[7];
T i = this->data[8];
return (-(c * e * g) + b * f * g + c * d * h - a * f * h - b * d * i +
a * e * i);
}
virtual inline MatrixNxNBase<T, 3> Inv() const {
if (math::fabs(Det()) < std::numeric_limits<T>::denorm_min())
throw std::runtime_error("Matrix3x3Base: appears singular");
T a = this->data[0];
T b = this->data[1];
T c = this->data[2];
T d = this->data[3];
T e = this->data[4];
T f = this->data[5];
T g = this->data[6];
T h = this->data[7];
T i = this->data[8];
T tmp[9] = {(f * h - e * i) / (c * e * g - b * f * g - c * d * h +
a * f * h + b * d * i - a * e * i),
(c * h - b * i) / (-(c * e * g) + b * f * g + c * d * h -
a * f * h - b * d * i + a * e * i),
(c * e - b * f) / (c * e * g - b * f * g - c * d * h +
a * f * h + b * d * i - a * e * i),
(f * g - d * i) / (-(c * e * g) + b * f * g + c * d * h -
a * f * h - b * d * i + a * e * i),
(c * g - a * i) / (c * e * g - b * f * g - c * d * h +
a * f * h + b * d * i - a * e * i),
(c * d - a * f) / (-(c * e * g) + b * f * g + c * d * h -
a * f * h - b * d * i + a * e * i),
(e * g - d * h) / (c * e * g - b * f * g - c * d * h +
a * f * h + b * d * i - a * e * i),
(b * g - a * h) / (-(c * e * g) + b * f * g + c * d * h -
a * f * h - b * d * i + a * e * i),
(b * d - a * e) / (c * e * g - b * f * g - c * d * h +
a * f * h + b * d * i - a * e * i)};
return MatrixNxNBase<T, 3>(tmp);
}
};
inline Matrix3x3Base<float> operator*(const float& lhs,
const Matrix3x3Base<float>& rhs) {
return Matrix3x3Base<float>(rhs.Scale(lhs));
}
inline Matrix3x3Base<double> operator*(const double& lhs,
const Matrix3x3Base<double>& rhs) {
return Matrix3x3Base<double>(rhs.Scale(lhs));
}
typedef Matrix3x3Base<float> Matrix3x3f;
typedef Matrix3x3Base<float> Mat33f;
typedef Matrix3x3Base<double> Matrix3x3d;
typedef Matrix3x3Base<double> Mat33d;
typedef Matrix3x3Base<double> Matrix3x3;
typedef Matrix3x3Base<double> Mat33;
}
#endif

240
Livox_automatic_calibration-master/include/geometry_utils/Matrix4x4.h Executable file
View File

@@ -0,0 +1,240 @@
#ifndef GEOMETRY_UTILS_MATRIX4X4_H
#define GEOMETRY_UTILS_MATRIX4X4_H
#include "MatrixNxNBase.h"
namespace geometry_utils {
template <typename T>
struct Matrix4x4Base : MatrixNxNBase<T, 4> {
Matrix4x4Base() : MatrixNxNBase<T, 4>() {}
Matrix4x4Base(T val) : MatrixNxNBase<T, 4>(val) {}
Matrix4x4Base(const Matrix4x4Base& in) : MatrixNxNBase<T, 4>(in.data) {}
Matrix4x4Base(const boost::array<T, 16>& in) : MatrixNxNBase<T, 4>(in) {}
Matrix4x4Base(T (&in)[16]) : MatrixNxNBase<T, 4>(in) {}
Matrix4x4Base(const Eigen::Matrix<T, 4, 4>& in) : MatrixNxNBase<T, 4>(in) {}
Matrix4x4Base(const MatrixNxNBase<T, 4>& in) : MatrixNxNBase<T, 4>(in) {}
Matrix4x4Base(const MatrixNxMBase<T, 4, 4>& in) : MatrixNxNBase<T, 4>(in) {}
Matrix4x4Base(T R11, T R12, T R13, T R14, T R21, T R22, T R23, T R24, T R31,
T R32, T R33, T R34, T R41, T R42, T R43, T R44) {
this->data[0] = R11;
this->data[1] = R12;
this->data[2] = R13;
this->data[3] = R14;
this->data[4] = R21;
this->data[5] = R22;
this->data[6] = R23;
this->data[7] = R24;
this->data[8] = R31;
this->data[9] = R32;
this->data[10] = R33;
this->data[11] = R34;
this->data[12] = R41;
this->data[13] = R42;
this->data[14] = R43;
this->data[15] = R44;
}
inline T Det() const {
T a = this->data[0];
T b = this->data[1];
T c = this->data[2];
T d = this->data[3];
T e = this->data[4];
T f = this->data[5];
T g = this->data[6];
T h = this->data[7];
T i = this->data[8];
T j = this->data[9];
T k = this->data[10];
T l = this->data[11];
T m = this->data[12];
T n = this->data[13];
T o = this->data[14];
T p = this->data[15];
return d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m +
c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n +
d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o +
b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p +
c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p;
}
virtual inline MatrixNxNBase<T, 4> Inv() const {
if (math::fabs(Det()) < std::numeric_limits<T>::denorm_min())
throw std::runtime_error("Matrix4x4Base: appears singular");
T a = this->data[0];
T b = this->data[1];
T c = this->data[2];
T d = this->data[3];
T e = this->data[4];
T f = this->data[5];
T g = this->data[6];
T h = this->data[7];
T i = this->data[8];
T j = this->data[9];
T k = this->data[10];
T l = this->data[11];
T m = this->data[12];
T n = this->data[13];
T o = this->data[14];
T p = this->data[15];
T tmp[16] = {
(-(h * k * n) + g * l * n + h * j * o - f * l * o - g * j * p +
f * k * p) /
(d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m +
c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n +
d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o +
b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p +
c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
(d * k * n - c * l * n - d * j * o + b * l * o + c * j * p -
b * k * p) /
(d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m +
c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n +
d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o +
b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p +
c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
(-(d * g * n) + c * h * n + d * f * o - b * h * o - c * f * p +
b * g * p) /
(d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m +
c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n +
d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o +
b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p +
c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
(d * g * j - c * h * j - d * f * k + b * h * k + c * f * l -
b * g * l) /
(d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m +
c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n +
d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o +
b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p +
c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
(h * k * m - g * l * m - h * i * o + e * l * o + g * i * p -
e * k * p) /
(d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m +
c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n +
d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o +
b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p +
c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
(-(d * k * m) + c * l * m + d * i * o - a * l * o - c * i * p +
a * k * p) /
(d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m +
c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n +
d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o +
b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p +
c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
(d * g * m - c * h * m - d * e * o + a * h * o + c * e * p -
a * g * p) /
(d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m +
c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n +
d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o +
b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p +
c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
(-(d * g * i) + c * h * i + d * e * k - a * h * k - c * e * l +
a * g * l) /
(d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m +
c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n +
d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o +
b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p +
c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
(-(h * j * m) + f * l * m + h * i * n - e * l * n - f * i * p +
e * j * p) /
(d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m +
c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n +
d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o +
b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p +
c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
(d * j * m - b * l * m - d * i * n + a * l * n + b * i * p -
a * j * p) /
(d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m +
c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n +
d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o +
b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p +
c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
(-(d * f * m) + b * h * m + d * e * n - a * h * n - b * e * p +
a * f * p) /
(d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m +
c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n +
d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o +
b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p +
c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
(d * f * i - b * h * i - d * e * j + a * h * j + b * e * l -
a * f * l) /
(d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m +
c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n +
d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o +
b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p +
c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
(g * j * m - f * k * m - g * i * n + e * k * n + f * i * o -
e * j * o) /
(d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m +
c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n +
d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o +
b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p +
c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
(-(c * j * m) + b * k * m + c * i * n - a * k * n - b * i * o +
a * j * o) /
(d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m +
c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n +
d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o +
b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p +
c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
(c * f * m - b * g * m - c * e * n + a * g * n + b * e * o -
a * f * o) /
(d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m +
c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n +
d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o +
b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p +
c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
(-(c * f * i) + b * g * i + c * e * j - a * g * j - b * e * k +
a * f * k) /
(d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m +
c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n +
d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o +
b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p +
c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p)};
return MatrixNxNBase<T, 4>(tmp);
}
};
inline Matrix4x4Base<float> operator*(const float& lhs,
const Matrix4x4Base<float>& rhs) {
return Matrix4x4Base<float>(rhs.Scale(lhs));
}
inline Matrix4x4Base<double> operator*(const double& lhs,
const Matrix4x4Base<double>& rhs) {
return Matrix4x4Base<double>(rhs.Scale(lhs));
}
typedef Matrix4x4Base<float> Matrix4x4f;
typedef Matrix4x4Base<float> Mat44f;
typedef Matrix4x4Base<double> Matrix4x4d;
typedef Matrix4x4Base<double> Mat44d;
typedef Matrix4x4Base<double> Matrix4x4;
typedef Matrix4x4Base<double> Mat44;
}
#endif

281
Livox_automatic_calibration-master/include/geometry_utils/MatrixNxMBase.h Executable file
View File

@@ -0,0 +1,281 @@
#ifndef GEOMETRY_UTILS_MATRIXNXM_H
#define GEOMETRY_UTILS_MATRIXNXM_H
#include <ostream>
#include "VectorNBase.h"
#include "GeometryUtilsMath.h"
namespace geometry_utils {
template <typename T, size_t N, size_t M>
struct MatrixNxMBase {
typedef typename boost::shared_ptr<MatrixNxMBase<T, N, M> > Ptr;
typedef typename boost::shared_ptr<const MatrixNxMBase<T, N, M> > ConstPtr;
static const size_t size = N * M;
static const size_t nrows = N;
static const size_t ncols = M;
boost::array<T, size> data;
MatrixNxMBase() { data.fill(0); }
MatrixNxMBase(T val) { data.fill(val); }
MatrixNxMBase(const MatrixNxMBase& in) : data(in.data) {}
MatrixNxMBase(const boost::array<T, size>& in) : data(in) {}
MatrixNxMBase(T (&in)[size]) {
for (unsigned int i = 0; i < size; i++)
data[i] = in[i];
}
MatrixNxMBase(const Eigen::Matrix<T, N, M>& in) {
for (size_t i = 0; i < nrows; i++)
for (size_t j = 0; j < ncols; j++)
data[ncols * i + j] = in(i, j);
}
inline T& operator()(unsigned int i) {
return data[i];
}
inline const T& operator()(unsigned int i) const {
return data[i];
}
inline T& Get(unsigned int i) {
return data[i];
}
inline const T& Get(unsigned int i) const {
return data[i];
}
inline T& operator()(unsigned int i, unsigned int j) {
return data[ncols * i + j];
}
inline const T& operator()(unsigned int i, unsigned int j) const {
return data[ncols * i + j];
}
inline T& Get(unsigned int i, unsigned int j) {
return data[ncols * i + j];
}
inline const T& Get(unsigned int i, unsigned int j) const {
return data[ncols * i + j];
}
inline std::ostream& operator<<(std::ostream& out) {
out << data;
return out;
}
inline MatrixNxMBase& operator=(const MatrixNxMBase& rhs) {
if (this == &rhs) return *this;
data = rhs.data;
return *this;
}
inline MatrixNxMBase<T, N, M> operator*(T rhs) const {
T d[size];
for (size_t i = 0; i < size; i++)
d[i] = data[i] * rhs;
return MatrixNxMBase<T, N, M>(d);
}
inline MatrixNxMBase<T, N, M> operator+(const MatrixNxMBase& rhs) const {
T d[size];
for (size_t i = 0; i < size; i++)
d[i] = data[i] + rhs(i);
return MatrixNxMBase<T, N, M>(d);
}
inline MatrixNxMBase<T, N, M> operator-() const {
T d[size];
for (size_t i = 0; i < size; i++)
d[i] = -data[i];
return MatrixNxMBase<T, N, M>(d);
}
inline MatrixNxMBase<T, N, M> operator-(const MatrixNxMBase& rhs) const {
T d[size];
for (size_t i = 0; i < size; i++)
d[i] = data[i] - rhs(i);
return MatrixNxMBase<T, N, M>(d);
}
inline MatrixNxMBase<T, N, N> operator*(const MatrixNxMBase<T, M, N>& rhs)
const {
T d[N * N];
for (size_t i = 0; i < N; i++)
for (size_t j = 0; j < N; j++)
d[N * i + j] = this->Row(i) ^ rhs.Col(j);
return MatrixNxMBase<T, N, N>(d);
}
inline VectorNBase<T, N> operator*(const VectorNBase<T, M>& rhs) const {
T d[nrows];
for (size_t i = 0; i < nrows; i++)
d[i] = this->Row(i) ^ rhs;
return VectorNBase<T, N>(d);
}
inline MatrixNxMBase<T, N, M> operator%(const MatrixNxMBase& rhs) const {
T d[size];
for (size_t i = 0; i < size; i++)
d[i] = data[i] * rhs(i);
return MatrixNxMBase<T, N, M>(d);
}
inline MatrixNxMBase<T, N, M> operator/(const MatrixNxMBase& rhs) const {
T d[size];
for (size_t i = 0; i < size; i++)
d[i] = data[i] / rhs(i);
return MatrixNxMBase<T, N, M>(d);
}
inline MatrixNxMBase<T, N, M> operator+=(const MatrixNxMBase& rhs) {
for (size_t i = 0; i < size; i++)
data[i] += rhs.data[i];
return *this;
}
inline MatrixNxMBase<T, N, M> operator-=(const MatrixNxMBase& rhs) {
for (size_t i = 0; i < size; i++)
data[i] -= rhs.data[i];
return *this;
}
inline MatrixNxMBase<T, N, M> operator%=(const MatrixNxMBase& rhs) {
for (size_t i = 0; i < size; i++)
data[i] *= rhs.data[i];
return *this;
}
inline MatrixNxMBase<T, N, M> operator/=(const MatrixNxMBase& rhs) {
for (size_t i = 0; i < size; i++)
data[i] /= rhs.data[i];
return *this;
}
inline MatrixNxMBase<T, M, N> Trans() const {
T d[size];
for (size_t i = 0; i < nrows; i++)
for (size_t j = 0; j < ncols; j++)
d[nrows * j + i] = data[ncols * i + j];
return MatrixNxMBase<T, M, N>(d);
}
inline MatrixNxMBase<T, M, N> t() const {
return this->Trans();
}
inline VectorNBase<T, M> Row(unsigned int r) const {
T d[ncols];
for (size_t i = 0; i < ncols; i++)
d[i] = data[ncols * r + i];
return VectorNBase<T, M>(d);
}
inline VectorNBase<T, N> Col(unsigned int c) const {
T d[nrows];
for (size_t i = 0; i < nrows; i++)
d[i] = data[ncols * i + c];
return VectorNBase<T, N>(d);
}
inline void Ones() { data.fill(1); }
inline void Zeros() { data.fill(0); }
inline MatrixNxMBase<T, N, M> Scale(T s) const {
return (*this) * s;
}
inline void Print(const std::string& prefix = std::string()) const {
if (!prefix.empty())
std::cout << prefix << std::endl;
std::cout << (*this) << std::endl;
}
inline Eigen::Matrix<T, N, M> Eigen() const {
return Eigen::Matrix<T, M, N>(data.data()).transpose();
}
inline bool operator==(const MatrixNxMBase& that) const {
return this->Equals(that);
}
inline bool operator!=(const MatrixNxMBase& that) const {
return !this->Equals(that);
}
virtual inline bool Equals(const MatrixNxMBase& that,
const T ptol = 1e-8) const {
return (*this - that).Norm() < ptol;
}
inline T Norm() const {
return math::sqrt((this->Trans() * (*this)).Trace());
}
inline T Trace() {
size_t count = nrows <= ncols ? nrows : ncols;
T tr = 0;
for (size_t i = 0; i < count; i++)
tr += data[ncols * i + i];
return tr;
}
};
template <size_t N, size_t M>
inline MatrixNxMBase<float, N, M> operator*(
const float& lhs, const MatrixNxMBase<float, N, M>& rhs) {
return rhs * lhs;
}
template <size_t N, size_t M>
inline MatrixNxMBase<double, N, M> operator*(
const double& lhs, const MatrixNxMBase<double, N, M>& rhs) {
return rhs * lhs;
}
template <typename T, size_t N, size_t M>
inline std::ostream& operator<<(std::ostream& out,
const MatrixNxMBase<T, N, M>& m) {
for (size_t i = 0; i < N; i++) {
for (size_t j = 0; j < M; j++)
out << m.data[M * i + j] << " ";
out << std::endl;
}
return out;
}
template <typename T, size_t N, size_t M>
inline Eigen::Matrix<T, N, M> Eigen(const MatrixNxMBase<T, N, M>& in) {
return in.Eigen();
}
template <typename T, size_t N, size_t M>
inline MatrixNxMBase<T, M, N> Trans(const MatrixNxMBase<T, N, M>& m) {
return m.Trans();
}
template <typename T, size_t N, size_t M>
inline MatrixNxMBase<T, N, M> Outer(const VectorNBase<T, N>& v1,
const VectorNBase<T, M>& v2) {
T d[N * M];
for (size_t i = 0; i < N; i++)
for (size_t j = 0; j < M; j++)
d[M * i + j] = v1(i) * v2(j);
return MatrixNxMBase<T, N, M>(d);
}
}
#endif

66
Livox_automatic_calibration-master/include/geometry_utils/MatrixNxNBase.h Executable file
View File

@@ -0,0 +1,66 @@
#ifndef GEOMETRY_UTILS_MATRIXNXN_H
#define GEOMETRY_UTILS_MATRIXNXN_H
#include <ostream>
#include "VectorNBase.h"
#include "GeometryUtilsMath.h"
#include "MatrixNxMBase.h"
namespace geometry_utils {
template <typename T, size_t N>
struct MatrixNxNBase : MatrixNxMBase<T, N, N> {
MatrixNxNBase() : MatrixNxMBase<T, N, N>() {}
MatrixNxNBase(T val) : MatrixNxMBase<T, N, N>(val) {}
MatrixNxNBase(const MatrixNxNBase& in) : MatrixNxMBase<T, N, N>(in.data) {}
MatrixNxNBase(const boost::array<T, N* N>& in) : MatrixNxMBase<T, N, N>(in) {}
MatrixNxNBase(T (&in)[N * N]) : MatrixNxMBase<T, N, N>(in) {}
MatrixNxNBase(const Eigen::Matrix<T, N, N>& in)
: MatrixNxMBase<T, N, N>(in) {}
MatrixNxNBase(const MatrixNxMBase<T, N, N>& in)
: MatrixNxMBase<T, N, N>(in) {}
inline void Eye() {
this->data.fill(0);
for (size_t i = 0; i < this->nrows; i++)
this->data[this->nrows * i + i] = 1;
}
virtual inline T Det() const {
std::cerr << "MatrixNxMBase::det not implemented" << std::endl;
return T();
}
virtual inline MatrixNxNBase<T, N> Inv() const {
std::cerr << "MatrixNxMBase::inv not implemented" << std::endl;
return MatrixNxNBase<T, N>();
}
static inline MatrixNxNBase<T, N> Diag(const VectorNBase<T, N>& in) {
T d[N * N] = {0};
for (size_t i = 0; i < N; i++)
d[N * i + i] = in(i);
return MatrixNxNBase<T, N>(d);
}
};
template <size_t N>
inline MatrixNxNBase<float, N> operator*(const float& lhs,
const MatrixNxNBase<float, N>& rhs) {
return MatrixNxNBase<float, N>(rhs * lhs);
}
template <size_t N>
inline MatrixNxNBase<double, N> operator*(const double& lhs,
const MatrixNxNBase<double, N>& rhs) {
return MatrixNxNBase<double, N>(rhs * lhs);
}
template <typename T, size_t N>
inline MatrixNxNBase<T, N> Inv(const MatrixNxNBase<T, N>& m) {
return m.Inv();
}
}
#endif

96
Livox_automatic_calibration-master/include/geometry_utils/Quaternion.h Executable file
View File

@@ -0,0 +1,96 @@
#ifndef GEOMETRY_UTILS_QUATERNION_H
#define GEOMETRY_UTILS_QUATERNION_H
#include <string>
#include <boost/array.hpp>
#include <Eigen/Geometry>
#include "GeometryUtilsMath.h"
namespace geometry_utils {
template <typename T>
struct QuaternionBase : VectorNBase<T, 4> {
QuaternionBase() : VectorNBase<T, 4>() {
this->data.assign(0);
this->data[0] = 1;
}
QuaternionBase(T val) : VectorNBase<T, 4>(val) {}
QuaternionBase(const QuaternionBase& in) : VectorNBase<T, 4>(in.data) {}
QuaternionBase(const boost::array<T, 4>& in) : VectorNBase<T, 4>(in) {}
QuaternionBase(T (&in)[4]) : VectorNBase<T, 4>(in) {}
QuaternionBase(const Eigen::Quaternion<T>& in) {
this->data[0] = in.w();
this->data[1] = in.x();
this->data[2] = in.y();
this->data[3] = in.z();
}
QuaternionBase(const VectorNBase<T, 4>& in) : VectorNBase<T, 4>(in) {}
QuaternionBase(T w, T x, T y, T z) {
this->data[0] = w;
this->data[1] = x;
this->data[2] = y;
this->data[3] = z;
}
inline QuaternionBase operator*(const QuaternionBase& rhs) const {
T a1 = this->data[0];
T b1 = this->data[1];
T c1 = this->data[2];
T d1 = this->data[3];
T a2 = rhs.data[0];
T b2 = rhs.data[1];
T c2 = rhs.data[2];
T d2 = rhs.data[3];
return QuaternionBase(a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2,
a2 * b1 + a1 * b2 - c2 * d1 + c1 * d2,
a2 * c1 + a1 * c2 + b2 * d1 - b1 * d2,
-(b2 * c1) + b1 * c2 + a2 * d1 + a1 * d2);
}
inline T& W() { return this->data[0]; }
inline const T& W() const { return this->data[0]; }
inline T& X() { return this->data[1]; }
inline const T& X() const { return this->data[1]; }
inline T& Y() { return this->data[2]; }
inline const T& Y() const { return this->data[2]; }
inline T& Z() { return this->data[3]; }
inline const T& Z() const { return this->data[3]; }
inline QuaternionBase Conj() const {
return QuaternionBase(this->data[0], -this->data[1], -this->data[2],
-this->data[3]);
}
inline QuaternionBase Error(const QuaternionBase& q) const {
return q * (*this).Conj();
}
inline QuaternionBase AxisAngle() const {
QuaternionBase q((*this));
if (q.W() > 1)
q = q.Normalize();
T den = math::sqrt(1 - q.W() * q.W());
if (den < 1e-6)
den = 1;
return QuaternionBase(2 * math::acos(q.W()), q.X() / den, q.Y() / den,
q.Z() / den);
}
};
typedef QuaternionBase<float> Quaternionf;
typedef QuaternionBase<float> Quatf;
typedef QuaternionBase<double> Quaterniond;
typedef QuaternionBase<double> Quatd;
typedef QuaternionBase<double> Quaternion;
typedef QuaternionBase<double> Quat;
}
#endif

84
Livox_automatic_calibration-master/include/geometry_utils/Rotation2.h Executable file
View File

@@ -0,0 +1,84 @@
#ifndef GEOMETRY_UTILS_ROTATION2_H
#define GEOMETRY_UTILS_ROTATION2_H
#include <Eigen/Geometry>
#include "GeometryUtilsMath.h"
#include "RotationNBase.h"
#include "Matrix2x2.h"
namespace geometry_utils {
template <typename T>
struct Rotation2Base : RotationNBase<T, 2> {
Rotation2Base() : RotationNBase<T, 2>() {}
Rotation2Base(const Rotation2Base& in) : RotationNBase<T, 2>(in.data) {}
Rotation2Base(const boost::array<T, 4>& in) : RotationNBase<T, 2>(in) {}
Rotation2Base(T (&in)[2 * 2]) : RotationNBase<T, 2>(in) {}
Rotation2Base(const Eigen::Matrix<T, 2, 2>& in) : RotationNBase<T, 2>(in) {}
Rotation2Base(const Eigen::Rotation2D<T>& in)
: RotationNBase<T, 2>(in.toRotationMatrix()) {}
Rotation2Base(const RotationNBase<T, 2>& in) : RotationNBase<T, 2>(in) {}
Rotation2Base(const Matrix2x2Base<T>& in) : RotationNBase<T, 2>(in) {}
Rotation2Base(const MatrixNxMBase<T, 2, 2>& in) : RotationNBase<T, 2>(in) {}
Rotation2Base(T val) { FromAngle(val); }
Rotation2Base(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 bool Equals(const Rotation2Base& that,
const T ptol = 1e-8) const {
return Error(that) < ptol;
}
inline T Error(const Rotation2Base& r) const {
return math::sin(Angle() - r.Angle());
}
inline T Angle() const {
return math::atan2(this->data[2], this->data[0]);
}
inline void FromAngle(T val) {
this->data[0] = math::cos(val);
this->data[1] = -math::sin(val);
this->data[2] = math::sin(val);
this->data[3] = math::cos(val);
}
inline Eigen::Rotation2D<T> Eigen() {
return Eigen::Rotation2D<T>(Angle());
}
};
inline Rotation2Base<float> operator*(const float& lhs,
const Rotation2Base<float>& rhs) {
return Rotation2Base<float>(rhs.Scale(lhs));
}
inline Rotation2Base<double> operator*(const double& lhs,
const Rotation2Base<double>& rhs) {
return Rotation2Base<double>(rhs.Scale(lhs));
}
template <typename T>
inline Eigen::Rotation2D<T> Eigen(const Rotation2Base<T>& in) {
return in.Eigen();
}
typedef Rotation2Base<float> Rotation2f;
typedef Rotation2Base<float> Rot2f;
typedef Rotation2Base<double> Rotation2d;
typedef Rotation2Base<double> Rot2d;
typedef Rotation2Base<double> Rotation2;
typedef Rotation2Base<double> Rot2;
}
#endif

195
Livox_automatic_calibration-master/include/geometry_utils/Rotation3.h Executable file
View File

@@ -0,0 +1,195 @@
#ifndef GEOMETRY_UTILS_ROTATION3_H
#define GEOMETRY_UTILS_ROTATION3_H
#include <Eigen/Geometry>
#include "GeometryUtilsMath.h"
#include "RotationNBase.h"
#include "Vector3.h"
#include "Quaternion.h"
#include "Rotation2.h"
#include "Matrix2x2.h"
namespace geometry_utils {
template <typename T>
struct Rotation3Base : RotationNBase<T, 3> {
Rotation3Base() : RotationNBase<T, 3>() {}
Rotation3Base(const Rotation3Base& in) : RotationNBase<T, 3>(in.data) {}
Rotation3Base(const boost::array<T, 9>& in) : RotationNBase<T, 3>(in) {}
Rotation3Base(T (&in)[9]) : RotationNBase<T, 3>(in) {}
Rotation3Base(const Eigen::Matrix<T, 3, 3>& in) : RotationNBase<T, 3>(in) {}
Rotation3Base(const Eigen::AngleAxis<T>& in)
: RotationNBase<T, 3>(in.toRotationMatrix()) {}
Rotation3Base(const RotationNBase<T, 3>& in) : RotationNBase<T, 3>(in) {}
Rotation3Base(const Matrix2x2Base<T>& in) : RotationNBase<T, 3>(in) {}
Rotation3Base(const MatrixNxMBase<T, 3, 3>& in) : RotationNBase<T, 3>(in) {}
Rotation3Base(T R11, T R12, T R13, T R21, T R22, T R23, T R31, T R32, T R33) {
this->data[0] = R11;
this->data[1] = R12;
this->data[2] = R13;
this->data[3] = R21;
this->data[4] = R22;
this->data[5] = R23;
this->data[6] = R31;
this->data[7] = R32;
this->data[8] = R33;
}
Rotation3Base(const Vector3Base<T>& in) {
FromEulerZYX(in(0), in(1), in(2));
}
Rotation3Base(T euler_x, T euler_y, T euler_z) {
FromEulerZYX(euler_x, euler_y, euler_z);
}
Rotation3Base(const Rotation2Base<T>& rot) {
this->Zeros();
this->data[0] = rot(0);
this->data[1] = rot(1);
this->data[3] = rot(2);
this->data[4] = rot(3);
this->data[8] = static_cast<T>(1);
}
Rotation3Base(const QuaternionBase<T>& quat) {
QuaternionBase<T> q(quat);
if (math::fabs(1 - q.Norm()) > static_cast<T>(1e-6))
q = q.Normalize();
T a = q.W();
T b = q.X();
T c = q.Y();
T d = q.Z();
T a2 = a * a;
T b2 = b * b;
T c2 = c * c;
T d2 = d * d;
T ab = a * b;
T ac = a * c;
T ad = a * d;
T bc = b * c;
T bd = b * d;
T cd = c * d;
(*this)(0, 0) = a2 + b2 - c2 - d2;
(*this)(0, 1) = static_cast<T>(2) * (bc - ad);
(*this)(0, 2) = static_cast<T>(2) * (bd + ac);
(*this)(1, 0) = static_cast<T>(2) * (bc + ad);
(*this)(1, 1) = a2 - b2 + c2 - d2;
(*this)(1, 2) = static_cast<T>(2) * (cd - ab);
(*this)(2, 0) = static_cast<T>(2) * (bd - ac);
(*this)(2, 1) = static_cast<T>(2) * (cd + ab);
(*this)(2, 2) = a2 - b2 - c2 + d2;
}
virtual inline bool Equals(const Rotation3Base& that,
const T ptol = 1e-8) const {
return Error(that) < ptol;
}
inline T Error(const Rotation3Base& r) const {
return 0; /*Norm(
static_cast<T>(0.5) *
Rotation3Base<T>(this->Trans() * r - r.Trans() * (*this)).Vee());*/
}
inline Vector3Base<T> Vee() const {
return Vector3Base<T>((*this)(2, 1), (*this)(0, 2), (*this)(1, 0));
}
inline void FromEulerZYX(T euler_x, T euler_y, T euler_z) {
T cph = math::cos(euler_x);
T sph = math::sin(euler_x);
T ct = math::cos(euler_y);
T st = math::sin(euler_y);
T cps = math::cos(euler_z);
T sps = math::sin(euler_z);
(*this)(0, 0) = ct * cps;
(*this)(0, 1) = cps * st * sph - cph * sps;
(*this)(0, 2) = cph * cps * st + sph * sps;
(*this)(1, 0) = ct * sps;
(*this)(1, 1) = cph * cps + st * sph * sps;
(*this)(1, 2) = -cps * sph + cph * st * sps;
(*this)(2, 0) = -st;
(*this)(2, 1) = ct * sph;
(*this)(2, 2) = ct * cph;
}
inline Vector3Base<T> GetEulerZYX() const {
return ToEulerZYX();
}
inline Vector3Base<T> ToEulerZYX() const {
T theta = -math::asin((*this)(2, 0));
T phi = 0;
T psi = 0;
if (math::fabs(cos(theta)) > static_cast<T>(1e-6)) {
phi = math::atan2((*this)(2, 1), (*this)(2, 2));
psi = math::atan2((*this)(1, 0), (*this)(0, 0));
}
return Vector3Base<T>(phi, theta, psi);
}
inline T Roll() const {
T theta = -math::asin((*this)(2, 0));
return math::fabs(cos(theta)) > static_cast<T>(1e-6)
? math::atan2((*this)(2, 1), (*this)(2, 2))
: 0;
}
inline T Pitch() const {
return -math::asin((*this)(2, 0));
}
inline T Yaw() const {
T theta = -math::asin((*this)(2, 0));
return math::fabs(cos(theta)) > static_cast<T>(1e-6)
? math::atan2((*this)(1, 0), (*this)(0, 0))
: 0;
}
};
inline Rotation3Base<float> operator*(const float& lhs,
const Rotation3Base<float>& rhs) {
return Rotation3Base<float>(rhs.Scale(lhs));
}
inline Rotation3Base<double> operator*(const double& lhs,
const Rotation3Base<double>& rhs) {
return Rotation3Base<double>(rhs.Scale(lhs));
}
template <typename T>
inline Rotation3Base<T> Hat(const Vector3Base<T>& v) {
return Rotation3Base<T>(0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0);
}
template <typename T>
inline Vector3Base<T> Vee(const MatrixNxMBase<T, 3, 3>& m) {
return Rotation3Base<T>(m).Vee();
}
typedef Rotation3Base<float> Rotation3f;
typedef Rotation3Base<float> Rot3f;
typedef Rotation3Base<double> Rotation3d;
typedef Rotation3Base<double> Rot3d;
typedef Rotation3Base<double> Rotation3;
typedef Rotation3Base<double> Rot3;
}
#endif

45
Livox_automatic_calibration-master/include/geometry_utils/RotationNBase.h Executable file
View File

@@ -0,0 +1,45 @@
#ifndef GEOMETRY_UTILS_ROTATIONN_H
#define GEOMETRY_UTILS_ROTATIONN_H
#include <ostream>
#include <Eigen/Core>
#include "VectorNBase.h"
#include "MatrixNxNBase.h"
namespace geometry_utils {
template <typename T, size_t N>
struct RotationNBase : MatrixNxNBase<T, N> {
RotationNBase() : MatrixNxNBase<T, N>() { this->Eye(); }
RotationNBase(const RotationNBase& in) : MatrixNxNBase<T, N>(in.data) {}
RotationNBase(const boost::array<T, N* N>& in) : MatrixNxNBase<T, N>(in) {}
RotationNBase(T (&in)[N * N]) : MatrixNxNBase<T, N>(in) {}
RotationNBase(const Eigen::Matrix<T, N, N>& in) : MatrixNxNBase<T, N>(in) {}
RotationNBase(const MatrixNxNBase<T, N>& in) : MatrixNxNBase<T, N>(in) {}
virtual inline MatrixNxNBase<T, N> Inv() const {
return this->Trans();
}
};
template <size_t N>
inline RotationNBase<float, N> operator*(const float& lhs,
const RotationNBase<float, N>& rhs) {
return RotationNBase<float, N>(rhs * lhs);
}
template <size_t N>
inline RotationNBase<double, N> operator*(const double& lhs,
const RotationNBase<double, N>& rhs) {
return RotationNBase<double, N>(rhs * lhs);
}
template <typename T, size_t N>
inline RotationNBase<T, N> Inv(const RotationNBase<T, N>& m) {
return m.Inv();
}
}
#endif

108
Livox_automatic_calibration-master/include/geometry_utils/Transform2.h Executable file
View File

@@ -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

116
Livox_automatic_calibration-master/include/geometry_utils/Transform3.h Executable file
View File

@@ -0,0 +1,116 @@
#ifndef GEOMETRY_UTILS_TRANSFORM3_H
#define GEOMETRY_UTILS_TRANSFORM3_H
#include "Vector3.h"
#include "Rotation3.h"
#include "Transform2.h"
namespace geometry_utils {
template <typename T>
struct Transform3Base {
typedef boost::shared_ptr<Transform3Base> Ptr;
typedef boost::shared_ptr<const Transform3Base> ConstPtr;
Vector3Base<T> translation;
Rotation3Base<T> rotation;
Transform3Base() {
translation.Zeros();
rotation.Eye();
}
Transform3Base(const Vector3Base<T>& translation_,
const Rotation3Base<T>& rotation_)
: translation(translation_), rotation(rotation_) {}
Transform3Base(const Transform3Base<T>& in)
: translation(in.translation), rotation(in.rotation) {}
Transform3Base(const Transform2Base<T>& in) {
translation(0) = in.translation(0);
translation(1) = in.translation(1);
translation(2) = 0;
rotation.Eye();
for (unsigned int i = 0; i < 2; i++)
for (unsigned int j = 0; j < 2; j++) rotation(i, j) = in.Rotation(i, j);
}
Transform3Base& operator=(const Transform3Base& rhs) {
if (this == &rhs)
return *this;
translation = rhs.translation;
rotation = rhs.rotation;
return *this;
}
Vector3Base<T> operator*(const Vector3Base<T>& p) const {
return rotation * p + translation;
}
Transform3Base<T> operator+(const Transform3Base<T>& t) const {
return Transform3Base<T>(translation + rotation * t.translation,
rotation * t.rotation);
}
bool operator==(const Transform3Base& that) const {
return this->Equals(that);
}
bool operator!=(const Transform3Base& that) const {
return !this->Equals(that);
}
bool Equals(const Transform3Base& 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 Transform3Base Identity() {
return Transform3Base();
}
};
template <typename T>
std::ostream& operator<<(std::ostream& out, const Transform3Base<T>& m) {
out << "Translation:" << std::endl << m.translation << std::endl;
out << "Rotation:" << std::endl << m.rotation;
return out;
}
template <typename T>
Transform3Base<T> PoseUpdate(const Transform3Base<T>& t1,
const Transform3Base<T>& t2) {
return Transform3Base<T>(t1.translation + t1.rotation * t2.translation,
t1.rotation * t2.rotation);
}
template <typename T>
Transform3Base<T> PoseInverse(const Transform3Base<T>& t) {
return Transform3Base<T>(-1.0 * t.rotation.Trans() * t.translation,
t.rotation.Trans());
}
template <typename T>
Transform3Base<T> PoseDelta(const Transform3Base<T>& t1,
const Transform3Base<T>& t2) {
return Transform3Base<T>(
t1.rotation.Trans() * (t2.translation - t1.translation),
t1.rotation.Trans() * t2.rotation);
}
typedef Transform3Base<float> Transform3f;
typedef Transform3Base<double> Transform3d;
typedef Transform3d Transform3;
typedef Transform3 Tr3;
}
#endif

48
Livox_automatic_calibration-master/include/geometry_utils/Vector2.h Executable file
View File

@@ -0,0 +1,48 @@
#ifndef GEOMETRY_UTILS_VECTOR2_H
#define GEOMETRY_UTILS_VECTOR2_H
#include "VectorNBase.h"
namespace geometry_utils {
template <typename T>
struct Vector2Base : VectorNBase<T, 2> {
Vector2Base() : VectorNBase<T, 2>() {}
Vector2Base(T val) : VectorNBase<T, 2>(val) {}
Vector2Base(const Vector2Base& in) : VectorNBase<T, 2>(in.data) {}
Vector2Base(const boost::array<T, 2>& in) : VectorNBase<T, 2>(in) {}
Vector2Base(T (&in)[2]) : VectorNBase<T, 2>(in) {}
Vector2Base(const Eigen::Matrix<T, 2, 1>& in) : VectorNBase<T, 2>(in) {}
Vector2Base(const VectorNBase<T, 2>& in) : VectorNBase<T, 2>(in) {}
Vector2Base(T v1, T v2) {
this->data[0] = v1;
this->data[1] = v2;
}
T X() const { return this->data[0]; }
T Y() const { return this->data[1]; }
};
inline Vector2Base<float> operator*(const float& lhs,
const Vector2Base<float>& rhs) {
return Vector2Base<float>(rhs * lhs);
}
inline Vector2Base<double> operator*(const double& lhs,
const Vector2Base<double>& rhs) {
return Vector2Base<double>(rhs * lhs);
}
typedef Vector2Base<float> Vector2f;
typedef Vector2Base<float> Vec2f;
typedef Vector2Base<double> Vector2d;
typedef Vector2Base<double> Vec2d;
typedef Vector2Base<double> Vector2;
typedef Vector2Base<double> Vec2;
}
#endif

62
Livox_automatic_calibration-master/include/geometry_utils/Vector3.h Executable file
View File

@@ -0,0 +1,62 @@
#ifndef GEOMETRY_UTILS_VECTOR3_H
#define GEOMETRY_UTILS_VECTOR3_H
#include "VectorNBase.h"
namespace geometry_utils {
template <typename T>
struct Vector3Base : VectorNBase<T, 3> {
Vector3Base() : VectorNBase<T, 3>() {}
Vector3Base(T val) : VectorNBase<T, 3>(val) {}
Vector3Base(const Vector3Base& in) : VectorNBase<T, 3>(in.data) {}
Vector3Base(const boost::array<T, 3>& in) : VectorNBase<T, 3>(in) {}
Vector3Base(T (&in)[3]) : VectorNBase<T, 3>(in) {}
Vector3Base(const Eigen::Matrix<T, 3, 1>& in) : VectorNBase<T, 3>(in) {}
Vector3Base(const VectorNBase<T, 3>& in) : VectorNBase<T, 3>(in) {}
Vector3Base(T v1, T v2, T v3) {
this->data[0] = v1;
this->data[1] = v2;
this->data[2] = v3;
}
T X() const { return this->data[0]; }
T Y() const { return this->data[1]; }
T Z() const { return this->data[2]; }
inline Vector3Base<T> Cross(const Vector3Base<T>& v) const {
return Vector3Base<T>(-(*this)(2) * v(1) + (*this)(1) * v(2),
(*this)(2) * v(0) - (*this)(0) * v(2),
-(*this)(1) * v(0) + (*this)(0) * v(1));
}
};
inline Vector3Base<float> operator*(const float& lhs,
const Vector3Base<float>& rhs) {
return Vector3Base<float>(rhs * lhs);
}
inline Vector3Base<double> operator*(const double& lhs,
const Vector3Base<double>& rhs) {
return Vector3Base<double>(rhs * lhs);
}
template <typename T>
inline VectorNBase<T, 3> Cross(const VectorNBase<T, 3>& v1,
const VectorNBase<T, 3>& v2) {
return Vector3Base<T>(v1).Cross(v2);
}
typedef Vector3Base<float> Vector3f;
typedef Vector3Base<float> Vec3f;
typedef Vector3Base<double> Vector3d;
typedef Vector3Base<double> Vec3d;
typedef Vector3Base<double> Vector3;
typedef Vector3Base<double> Vec3;
}
#endif

47
Livox_automatic_calibration-master/include/geometry_utils/Vector4.h Executable file
View File

@@ -0,0 +1,47 @@
#ifndef GEOMETRY_UTILS_VECTOR4_H
#define GEOMETRY_UTILS_VECTOR4_H
#include "VectorNBase.h"
namespace geometry_utils {
template <typename T>
struct Vector4Base : VectorNBase<T, 4> {
Vector4Base() : VectorNBase<T, 4>() {}
Vector4Base(T val) : VectorNBase<T, 4>(val) {}
Vector4Base(const Vector4Base& in) : VectorNBase<T, 4>(in.data) {}
Vector4Base(const boost::array<T, 4>& in) : VectorNBase<T, 4>(in) {}
Vector4Base(T (&in)[4]) : VectorNBase<T, 4>(in) {}
Vector4Base(const Eigen::Matrix<T, 4, 1>& in) : VectorNBase<T, 4>(in) {}
Vector4Base(const VectorNBase<T, 4>& in) : VectorNBase<T, 4>(in) {}
Vector4Base(T v1, T v2, T v3, T v4) {
this->data[0] = v1;
this->data[1] = v2;
this->data[2] = v3;
this->data[3] = v4;
}
};
inline Vector4Base<float> operator*(const float& lhs,
const Vector4Base<float>& rhs) {
return Vector4Base<float>(rhs * lhs);
}
inline Vector4Base<double> operator*(const double& lhs,
const Vector4Base<double>& rhs) {
return Vector4Base<double>(rhs * lhs);
}
typedef Vector4Base<float> Vector4f;
typedef Vector4Base<float> Vec4f;
typedef Vector4Base<double> Vector4d;
typedef Vector4Base<double> Vec4d;
typedef Vector4Base<double> Vector4;
typedef Vector4Base<double> Vec4;
}
#endif

228
Livox_automatic_calibration-master/include/geometry_utils/VectorNBase.h Executable file
View File

@@ -0,0 +1,228 @@
#ifndef GEOMETRY_UTILS_VECTORN_H
#define GEOMETRY_UTILS_VECTORN_H
#include <iostream>
#include <string>
#include <boost/shared_ptr.hpp>
#include <boost/array.hpp>
#include "GeometryUtilsMath.h"
namespace geometry_utils {
template <typename T, size_t N>
struct VectorNBase {
typedef typename boost::shared_ptr<VectorNBase<T, N> > Ptr;
typedef typename boost::shared_ptr<const VectorNBase<T, N> > ConstPtr;
static const size_t length = N;
boost::array<T, N> data;
VectorNBase() { data.fill(0); }
VectorNBase(T val) { data.fill(val); }
VectorNBase(const VectorNBase& in) : data(in.data) {}
VectorNBase(const boost::array<T, N>& in) : data(in) {}
VectorNBase(T (&in)[N]) {
for (size_t i = 0; i < N; i++)
data[i] = in[i];
}
VectorNBase(const Eigen::Matrix<T, N, 1>& in) {
for (size_t i = 0; i < N; i++)
data[i] = in(i, 0);
}
inline T& operator()(unsigned int i) {
return data[i];
}
inline const T& operator()(unsigned int i) const {
return data[i];
}
inline T& Get(unsigned int i) {
return data[i];
}
inline const T& Get(unsigned int i) const {
return data[i];
}
inline VectorNBase& operator=(const VectorNBase& rhs) {
if (this == &rhs)
return *this;
data = rhs.data;
return *this;
}
inline VectorNBase operator*(T rhs) const {
T d[N];
for (size_t i = 0; i < N; i++)
d[i] = data[i] * rhs;
return VectorNBase<T, N>(d);
}
inline VectorNBase operator+(const VectorNBase& rhs) const {
T d[N];
for (size_t i = 0; i < N; i++)
d[i] = data[i] + rhs.data[i];
return VectorNBase<T, N>(d);
}
inline VectorNBase operator-() const {
T d[N];
for (size_t i = 0; i < N; i++)
d[i] = -data[i];
return VectorNBase<T, N>(d);
}
inline VectorNBase operator-(const VectorNBase& rhs) const {
T d[N];
for (size_t i = 0; i < N; i++)
d[i] = data[i] - rhs.data[i];
return VectorNBase<T, N>(d);
}
inline VectorNBase operator%(const VectorNBase& rhs) const {
T d[N];
for (size_t i = 0; i < N; i++)
d[i] = data[i] * rhs.data[i];
return VectorNBase<T, N>(d);
}
inline T operator^(const VectorNBase& rhs) const {
T dot = 0;
for (size_t i = 0; i < N; i++)
dot += data[i] * rhs.data[i];
return dot;
}
inline VectorNBase operator/(const VectorNBase& rhs) const {
T d[N];
for (size_t i = 0; i < N; i++)
d[i] = data[i] / rhs.data[i];
return VectorNBase<T, N>(d);
}
inline VectorNBase operator+=(const VectorNBase& rhs) {
for (size_t i = 0; i < N; i++)
data[i] += rhs.data[i];
return *this;
}
inline VectorNBase operator-=(const VectorNBase& rhs) {
for (size_t i = 0; i < N; i++)
data[i] -= rhs.data[i];
return *this;
}
inline VectorNBase operator%=(const VectorNBase& rhs) {
for (size_t i = 0; i < N; i++)
data[i] *= rhs.data[i];
return *this;
}
inline VectorNBase operator/=(const VectorNBase& rhs) {
for (size_t i = 0; i < N; i++)
data[i] /= rhs.data[i];
return *this;
}
inline VectorNBase operator*=(const T& rhs) {
for (size_t i = 0; i < N; i++)
data[i] *= rhs;
return *this;
}
inline VectorNBase operator/=(const T& rhs) {
for (size_t i = 0; i < N; i++)
data[i] /= rhs;
return *this;
}
inline bool operator==(const VectorNBase& that) const {
return this->Equals(that);
}
inline bool operator!=(const VectorNBase& that) const {
return !this->Equals(that);
}
inline bool Equals(const VectorNBase& that, const T ptol = 1e-8) const {
return (*this - that).Norm() < ptol;
}
inline T Norm() const { return math::sqrt((*this) ^ (*this)); }
inline VectorNBase Normalize() const { return (*this) / Norm(); }
inline VectorNBase Abs() const {
T d[N];
for (size_t i = 0; i < N; i++)
d[i] = std::abs(data[i]);
return VectorNBase<T, N>(d);
}
inline void Ones() {
data.fill(1);
}
inline void Zeros() {
data.fill(0);
}
inline T Dot(const VectorNBase& v) const {
return (*this) ^ v;
}
inline VectorNBase Scale(T s) const {
return (*this) * s;
}
inline void Print(const std::string& prefix = std::string()) const {
if (!prefix.empty())
std::cout << prefix << std::endl;
std::cout << (*this) << std::endl;
}
inline Eigen::Matrix<T, N, 1> Eigen() const {
return Eigen::Matrix<T, N, 1>(data.data());
}
};
template <typename T, size_t N>
inline VectorNBase<T, N> operator*(const T& lhs, const VectorNBase<T, N>& rhs) {
return rhs * lhs;
}
template <typename T, size_t N>
inline std::ostream& operator<<(std::ostream& out, const VectorNBase<T, N>& m) {
for (size_t i = 0; i < N - 1; i++)
out << m.data[i] << " ";
out << m.data[N - 1];
return out;
}
template <typename T, size_t N>
inline Eigen::Matrix<T, N, 1> Eigen(const VectorNBase<T, N>& in) {
return in.Eigen();
}
template <typename T, size_t N>
inline T Norm(const VectorNBase<T, N>& v) {
return v.Norm();
}
template <typename T, size_t N>
inline T Dot(const VectorNBase<T, N>& v1, const VectorNBase<T, N>& v2) {
return v1.Dot(v2);
}
}
#endif

98
Livox_automatic_calibration-master/include/geometry_utils/YAMLLoader.h Executable file
View File

@@ -0,0 +1,98 @@
/*
geometry_utils: Utility library to provide common geometry types and transformations
Copyright (C) 2013 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_YAML_LOADER_H
#define GEOMETRY_UTILS_YAML_LOADER_H
#ifdef __YAML_UTILS_H__
#error "YAMLUtils must be included last"
#endif
#include <yaml-cpp/yaml.h>
#include <geometry_utils/GeometryUtils.h>
namespace YAML {
template <>
struct convert<geometry_utils::Vector2> {
static Node Encode(const geometry_utils::Vector2& rhs) {
Node node;
node.push_back(rhs[0]);
node.push_back(rhs[1]);
return node;
}
static bool Decode(const Node& node, geometry_utils::Vector2& rhs) {
if (!node.IsSequence() || node.size() != 2)
return false;
rhs[0] = node[0].as<double>();
rhs[1] = node[1].as<double>();
return true;
}
};
template <>
struct convert<geometry_utils::Vector3> {
static Node Encode(const geometry_utils::Vector3& rhs) {
Node node;
node.push_back(rhs[0]);
node.push_back(rhs[1]);
node.push_back(rhs[2]);
return node;
}
static bool Decode(const Node& node, geometry_utils::Vector3& rhs) {
if (!node.IsSequence() || node.size() != 3)
return false;
rhs[0] = node[0].as<double>();
rhs[1] = node[1].as<double>();
rhs[2] = node[2].as<double>();
return true;
}
};
template <>
struct convert<geometry_utils::Vector4> {
static Node Encode(const geometry_utils::Vector4& rhs) {
Node node;
node.push_back(rhs[0]);
node.push_back(rhs[1]);
node.push_back(rhs[2]);
node.push_back(rhs[3]);
return node;
}
static bool Decode(const Node& node, geometry_utils::Vector4& rhs) {
if (!node.IsSequence() || node.size() != 4)
return false;
rhs[0] = node[0].as<double>();
rhs[1] = node[1].as<double>();
rhs[2] = node[2].as<double>();
rhs[3] = node[3].as<double>();
return true;
}
};
}
#endif