mirrors/FAST_LIO_LOCALIZATION
1
0
Fork 0
mirror of https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION.git synced 2023-05-28 12:51:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

fast_lio2 released!

Browse Source
This commit is contained in:
xw
2021-07-04 10:17:41 -04:00
parent 6e1fa94351
commit 1087a72497
44 changed files with 7155 additions and 2973 deletions
Download Patch File Download Diff File Expand all files Collapse all files

163
include/common_lib.h
View File

@@ -5,66 +5,76 @@
#include <Eigen/Eigen>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <fast_lio/States.h>
#include <fast_lio/Pose6D.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
#include <eigen_conversions/eigen_msg.h>
using namespace std;
using namespace Eigen;
// #define DEBUG_PRINT
// #define USE_ikdtree
#define USE_IKFOM
#define PI_M (3.14159265358)
#define G_m_s2 (9.8099) // Gravaty const in GuangDong/China
#define DIM_OF_STATES (18) // Dimension of states (Let Dim(SO(3)) = 3)
#define DIM_OF_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3)
#define G_m_s2 (9.81) // Gravaty const in GuangDong/China
#define DIM_STATE (18) // Dimension of states (Let Dim(SO(3)) = 3)
#define DIM_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3)
#define CUBE_LEN (6.0)
#define LIDAR_SP_LEN (2)
#define INIT_COV (0.0001)
#define INIT_COV (1)
#define NUM_MATCH_POINTS (5)
#define MAX_MEAS_DIM (10000)
#define VEC_FROM_ARRAY(v) v[0],v[1],v[2]
#define MAT_FROM_ARRAY(v) v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7],v[8]
#define CONSTRAIN(v,min,max) ((v>min)?((v<max)?v:max):min)
#define ARRAY_FROM_EIGEN(mat) mat.data(), mat.data() + mat.rows() * mat.cols()
#define STD_VEC_FROM_EIGEN(mat) std::vector<decltype(mat)::Scalar> (mat.data(), mat.data() + mat.rows() * mat.cols())
#define DEBUG_FILE_DIR(name) (std::string(std::string(ROOT_DIR) + "Log/"+ name))
#define STD_VEC_FROM_EIGEN(mat) vector<decltype(mat)::Scalar> (mat.data(), mat.data() + mat.rows() * mat.cols())
#define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/"+ name))
typedef fast_lio::Pose6D Pose6D;
typedef pcl::PointXYZINormal PointType;
typedef pcl::PointCloud<PointType> PointCloudXYZI;
typedef vector<PointType, Eigen::aligned_allocator<PointType>> PointVector;
typedef Vector3d V3D;
typedef Matrix3d M3D;
typedef Vector3f V3F;
typedef Matrix3f M3F;
Eigen::Matrix3d Eye3d(Eigen::Matrix3d::Identity());
Eigen::Matrix3f Eye3f(Eigen::Matrix3f::Identity());
Eigen::Vector3d Zero3d(0, 0, 0);
Eigen::Vector3f Zero3f(0, 0, 0);
// Eigen::Vector3d Lidar_offset_to_IMU(0.05512, 0.02226, 0.0297); // Horizon
Eigen::Vector3d Lidar_offset_to_IMU(0.04165, 0.02326, -0.0284); // Avia
#define MD(a,b) Matrix<double, (a), (b)>
#define VD(a) Matrix<double, (a), 1>
#define MF(a,b) Matrix<float, (a), (b)>
#define VF(a) Matrix<float, (a), 1>
M3D Eye3d(M3D::Identity());
M3F Eye3f(M3F::Identity());
V3D Zero3d(0, 0, 0);
V3F Zero3f(0, 0, 0);
struct MeasureGroup // Lidar data and imu dates for the curent process
{
MeasureGroup()
{
lidar_beg_time = 0.0;
this->lidar.reset(new PointCloudXYZI());
};
double lidar_beg_time;
PointCloudXYZI::Ptr lidar;
std::deque<sensor_msgs::Imu::ConstPtr> imu;
deque<sensor_msgs::Imu::ConstPtr> imu;
};
struct StatesGroup
{
StatesGroup() {
this->rot_end = Eigen::Matrix3d::Identity();
this->rot_end = M3D::Identity();
this->pos_end = Zero3d;
this->vel_end = Zero3d;
this->bias_g = Zero3d;
this->bias_a = Zero3d;
this->gravity = Zero3d;
this->cov = Eigen::Matrix<double,DIM_OF_STATES,DIM_OF_STATES>::Identity() * INIT_COV;
this->cov = MD(DIM_STATE,DIM_STATE)::Identity() * INIT_COV;
this->cov.block<9,9>(9,9) = MD(9,9)::Identity() * 0.00001;
};
StatesGroup(const StatesGroup& b) {
@@ -89,8 +99,7 @@ struct StatesGroup
return *this;
};
StatesGroup operator+(const Eigen::Matrix<double, DIM_OF_STATES, 1> &state_add)
StatesGroup operator+(const Matrix<double, DIM_STATE, 1> &state_add)
{
StatesGroup a;
a.rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0));
@@ -103,7 +112,7 @@ struct StatesGroup
return a;
};
StatesGroup& operator+=(const Eigen::Matrix<double, DIM_OF_STATES, 1> &state_add)
StatesGroup& operator+=(const Matrix<double, DIM_STATE, 1> &state_add)
{
this->rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0));
this->pos_end += state_add.block<3,1>(3,0);
@@ -114,10 +123,10 @@ struct StatesGroup
return *this;
};
Eigen::Matrix<double, DIM_OF_STATES, 1> operator-(const StatesGroup& b)
Matrix<double, DIM_STATE, 1> operator-(const StatesGroup& b)
{
Eigen::Matrix<double, DIM_OF_STATES, 1> a;
Eigen::Matrix3d rotd(b.rot_end.transpose() * this->rot_end);
Matrix<double, DIM_STATE, 1> a;
M3D rotd(b.rot_end.transpose() * this->rot_end);
a.block<3,1>(0,0) = Log(rotd);
a.block<3,1>(3,0) = this->pos_end - b.pos_end;
a.block<3,1>(6,0) = this->vel_end - b.vel_end;
@@ -127,13 +136,20 @@ struct StatesGroup
return a;
};
Eigen::Matrix3d rot_end; // the estimated attitude (rotation matrix) at the end lidar point
Eigen::Vector3d pos_end; // the estimated position at the end lidar point (world frame)
Eigen::Vector3d vel_end; // the estimated velocity at the end lidar point (world frame)
Eigen::Vector3d bias_g; // gyroscope bias
Eigen::Vector3d bias_a; // accelerator bias
Eigen::Vector3d gravity; // the estimated gravity acceleration
Eigen::Matrix<double, DIM_OF_STATES, DIM_OF_STATES> cov; // states covariance
void resetpose()
{
this->rot_end = M3D::Identity();
this->pos_end = Zero3d;
this->vel_end = Zero3d;
}
M3D rot_end; // the estimated attitude (rotation matrix) at the end lidar point
V3D pos_end; // the estimated position at the end lidar point (world frame)
V3D vel_end; // the estimated velocity at the end lidar point (world frame)
V3D bias_g; // gyroscope bias
V3D bias_a; // accelerator bias
V3D gravity; // the estimated gravity acceleration
Matrix<double, DIM_STATE, DIM_STATE> cov; // states covariance
};
template<typename T>
@@ -149,8 +165,8 @@ T deg2rad(T degrees)
}
template<typename T>
auto set_pose6d(const double t, const Eigen::Matrix<T, 3, 1> &a, const Eigen::Matrix<T, 3, 1> &g, \
const Eigen::Matrix<T, 3, 1> &v, const Eigen::Matrix<T, 3, 1> &p, const Eigen::Matrix<T, 3, 3> &R)
auto set_pose6d(const double t, const Matrix<T, 3, 1> &a, const Matrix<T, 3, 1> &g, \
const Matrix<T, 3, 1> &v, const Matrix<T, 3, 1> &p, const Matrix<T, 3, 3> &R)
{
Pose6D rot_kp;
rot_kp.offset_time = t;
@@ -162,10 +178,81 @@ auto set_pose6d(const double t, const Eigen::Matrix<T, 3, 1> &a, const Eigen::Ma
rot_kp.pos[i] = p(i);
for (int j = 0; j < 3; j++) rot_kp.rot[i*3+j] = R(i,j);
}
// Eigen::Map<Eigen::Matrix3d>(rot_kp.rot, 3,3) = R;
return std::move(rot_kp);
return move(rot_kp);
}
/* comment
plane equation: Ax + By + Cz + D = 0
convert to: A/D*x + B/D*y + C/D*z = -1
solve: A0*x0 = b0
where A0_i = [x_i, y_i, z_i], x0 = [A/D, B/D, C/D]^T, b0 = [-1, ..., -1]^T
normvec: normalized x0
*/
template<typename T>
bool esti_normvector(Matrix<T, 3, 1> &normvec, const PointVector &point, const T &threshold, const int &point_num)
{
MatrixXf A(point_num, 3);
MatrixXf b(point_num, 1);
b.setOnes();
b *= -1.0f;
for (int j = 0; j < point_num; j++)
{
A(j,0) = point[j].x;
A(j,1) = point[j].y;
A(j,2) = point[j].z;
}
normvec = A.colPivHouseholderQr().solve(b);
for (int j = 0; j < point_num; j++)
{
if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > threshold)
{
return false;
}
}
#endif
normvec.normalize();
return true;
}
float calc_dist(PointType p1, PointType p2){
float d = (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z);
return d;
}
template<typename T>
bool esti_plane(Matrix<T, 4, 1> &pca_result, const PointVector &point, const T &threshold)
{
Matrix<T, NUM_MATCH_POINTS, 3> A;
Matrix<T, NUM_MATCH_POINTS, 1> b;
A.setZero();
b.setOnes();
b *= -1.0f;
for (int j = 0; j < NUM_MATCH_POINTS; j++)
{
A(j,0) = point[j].x;
A(j,1) = point[j].y;
A(j,2) = point[j].z;
}
Matrix<T, 3, 1> normvec = A.colPivHouseholderQr().solve(b);
T n = normvec.norm();
pca_result(0) = normvec(0) / n;
pca_result(1) = normvec(1) / n;
pca_result(2) = normvec(2) / n;
pca_result(3) = 1.0 / n;
for (int j = 0; j < NUM_MATCH_POINTS; j++)
{
if (fabs(pca_result(0) * point[j].x + pca_result(1) * point[j].y + pca_result(2) * point[j].z + pca_result(3)) > threshold)
{
return false;
}
}
return true;
}
#endif