m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

feat(slam): add lidar SLAM and pointcloud processing packages

Browse Source
This commit is contained in:
X-lanni
2026-01-13 19:12:25 +08:00
parent dcb52622eb
commit ddef31d11a
140 changed files with 29546 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
point_lio_ros2/src/Estimator.cpp
+435
View File
@@ -0,0 +1,435 @@
// #include <../include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp>
#include "Estimator.h"
PointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1));
std::vector<int> time_seq;
PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI());
PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI());
std::vector<V3D> pbody_list;
std::vector<PointVector> Nearest_Points;
KD_TREE<PointType> ikdtree;
std::vector<float> pointSearchSqDis(NUM_MATCH_POINTS);
bool point_selected_surf[100000] = {0};
std::vector<M3D> crossmat_list;
int effct_feat_num = 0;
int k;
int idx;
esekfom::esekf<state_input, 24, input_ikfom> kf_input;
esekfom::esekf<state_output, 30, input_ikfom> kf_output;
state_input state_in;
state_output state_out;
input_ikfom input_in;
V3D angvel_avr, acc_avr;
V3D Lidar_T_wrt_IMU(Zero3d);
M3D Lidar_R_wrt_IMU(Eye3d);
typedef MTK::vect<3, double> vect3;
typedef MTK::SO3<double> SO3;
typedef MTK::S2<double, 98090, 10000, 1> S2;
typedef MTK::vect<1, double> vect1;
typedef MTK::vect<2, double> vect2;
Eigen::Matrix<double, 24, 24> process_noise_cov_input()
{
Eigen::Matrix<double, 24, 24> cov;
cov.setZero();
cov.block<3, 3>(3, 3).diagonal() << gyr_cov_input, gyr_cov_input, gyr_cov_input;
cov.block<3, 3>(12, 12).diagonal() << acc_cov_input, acc_cov_input, acc_cov_input;
cov.block<3, 3>(15, 15).diagonal() << b_gyr_cov, b_gyr_cov, b_gyr_cov;
cov.block<3, 3>(18, 18).diagonal() << b_acc_cov, b_acc_cov, b_acc_cov;
// MTK::get_cov<process_noise_input>::type cov = MTK::get_cov<process_noise_input>::type::Zero();
// MTK::setDiagonal<process_noise_input, vect3, 0>(cov, &process_noise_input::ng, gyr_cov_input);// 0.03
// MTK::setDiagonal<process_noise_input, vect3, 3>(cov, &process_noise_input::na, acc_cov_input); // *dt 0.01 0.01 * dt * dt 0.05
// MTK::setDiagonal<process_noise_input, vect3, 6>(cov, &process_noise_input::nbg, b_gyr_cov); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01
// MTK::setDiagonal<process_noise_input, vect3, 9>(cov, &process_noise_input::nba, b_acc_cov); //0.001 0.05 0.0001/out 0.01
return cov;
}
Eigen::Matrix<double, 30, 30> process_noise_cov_output()
{
Eigen::Matrix<double, 30, 30> cov;
cov.setZero();
cov.block<3, 3>(12, 12).diagonal() << vel_cov, vel_cov, vel_cov;
cov.block<3, 3>(15, 15).diagonal() << gyr_cov_output, gyr_cov_output, gyr_cov_output;
cov.block<3, 3>(18, 18).diagonal() << acc_cov_output, acc_cov_output, acc_cov_output;
cov.block<3, 3>(24, 24).diagonal() << b_gyr_cov, b_gyr_cov, b_gyr_cov;
cov.block<3, 3>(27, 27).diagonal() << b_acc_cov, b_acc_cov, b_acc_cov;
// MTK::get_cov<process_noise_output>::type cov = MTK::get_cov<process_noise_output>::type::Zero();
// MTK::setDiagonal<process_noise_output, vect3, 0>(cov, &process_noise_output::vel, vel_cov);// 0.03
// MTK::setDiagonal<process_noise_output, vect3, 3>(cov, &process_noise_output::ng, gyr_cov_output); // *dt 0.01 0.01 * dt * dt 0.05
// MTK::setDiagonal<process_noise_output, vect3, 6>(cov, &process_noise_output::na, acc_cov_output); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01
// MTK::setDiagonal<process_noise_output, vect3, 9>(cov, &process_noise_output::nbg, b_gyr_cov); //0.001 0.05 0.0001/out 0.01
// MTK::setDiagonal<process_noise_output, vect3, 12>(cov, &process_noise_output::nba, b_acc_cov); //0.001 0.05 0.0001/out 0.01
return cov;
}
Eigen::Matrix<double, 24, 1> get_f_input(state_input &s, const input_ikfom &in)
{
Eigen::Matrix<double, 24, 1> res = Eigen::Matrix<double, 24, 1>::Zero();
vect3 omega;
in.gyro.boxminus(omega, s.bg);
vect3 a_inertial = s.rot.normalized() * (in.acc-s.ba);
for(int i = 0; i < 3; i++ ){
res(i) = s.vel[i];
res(i + 3) = omega[i];
res(i + 12) = a_inertial[i] + s.gravity[i];
}
return res;
}
Eigen::Matrix<double, 30, 1> get_f_output(state_output &s, const input_ikfom &in)
{
Eigen::Matrix<double, 30, 1> res = Eigen::Matrix<double, 30, 1>::Zero();
vect3 a_inertial = s.rot.normalized() * s.acc;
for(int i = 0; i < 3; i++ ){
res(i) = s.vel[i];
res(i + 3) = s.omg[i];
res(i + 12) = a_inertial[i] + s.gravity[i];
}
return res;
}
Eigen::Matrix<double, 24, 24> df_dx_input(state_input &s, const input_ikfom &in)
{
Eigen::Matrix<double, 24, 24> cov = Eigen::Matrix<double, 24, 24>::Zero();
cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
vect3 acc_;
in.acc.boxminus(acc_, s.ba);
vect3 omega;
in.gyro.boxminus(omega, s.bg);
cov.template block<3, 3>(12, 3) = -s.rot.normalized().toRotationMatrix()*MTK::hat(acc_);
cov.template block<3, 3>(12, 18) = -s.rot.normalized().toRotationMatrix();
// Eigen::Matrix<state_ikfom::scalar, 2, 1> vec = Eigen::Matrix<state_ikfom::scalar, 2, 1>::Zero();
// Eigen::Matrix<state_ikfom::scalar, 3, 2> grav_matrix;
// s.S2_Mx(grav_matrix, vec, 21);
cov.template block<3, 3>(12, 21) = Eigen::Matrix3d::Identity(); // grav_matrix;
cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity();
return cov;
}
// Eigen::Matrix<double, 24, 12> df_dw_input(state_input &s, const input_ikfom &in)
// {
// Eigen::Matrix<double, 24, 12> cov = Eigen::Matrix<double, 24, 12>::Zero();
// cov.template block<3, 3>(12, 3) = -s.rot.normalized().toRotationMatrix();
// cov.template block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity();
// cov.template block<3, 3>(15, 6) = Eigen::Matrix3d::Identity();
// cov.template block<3, 3>(18, 9) = Eigen::Matrix3d::Identity();
// return cov;
// }
Eigen::Matrix<double, 30, 30> df_dx_output(state_output &s, const input_ikfom &in)
{
Eigen::Matrix<double, 30, 30> cov = Eigen::Matrix<double, 30, 30>::Zero();
cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
cov.template block<3, 3>(12, 3) = -s.rot.normalized().toRotationMatrix()*MTK::hat(s.acc);
cov.template block<3, 3>(12, 18) = s.rot.normalized().toRotationMatrix();
// Eigen::Matrix<state_ikfom::scalar, 2, 1> vec = Eigen::Matrix<state_ikfom::scalar, 2, 1>::Zero();
// Eigen::Matrix<state_ikfom::scalar, 3, 2> grav_matrix;
// s.S2_Mx(grav_matrix, vec, 21);
cov.template block<3, 3>(12, 21) = Eigen::Matrix3d::Identity(); // grav_matrix;
cov.template block<3, 3>(3, 15) = Eigen::Matrix3d::Identity();
return cov;
}
// Eigen::Matrix<double, 30, 15> df_dw_output(state_output &s)
// {
// Eigen::Matrix<double, 30, 15> cov = Eigen::Matrix<double, 30, 15>::Zero();
// cov.template block<3, 3>(12, 0) = Eigen::Matrix3d::Identity();
// cov.template block<3, 3>(15, 3) = Eigen::Matrix3d::Identity();
// cov.template block<3, 3>(18, 6) = Eigen::Matrix3d::Identity();
// cov.template block<3, 3>(24, 9) = Eigen::Matrix3d::Identity();
// cov.template block<3, 3>(27, 12) = Eigen::Matrix3d::Identity();
// return cov;
// }
vect3 SO3ToEuler(const SO3 &orient)
{
Eigen::Matrix<double, 3, 1> _ang;
Eigen::Vector4d q_data = orient.coeffs().transpose();
//scalar w=orient.coeffs[3], x=orient.coeffs[0], y=orient.coeffs[1], z=orient.coeffs[2];
double sqw = q_data[3]*q_data[3];
double sqx = q_data[0]*q_data[0];
double sqy = q_data[1]*q_data[1];
double sqz = q_data[2]*q_data[2];
double unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise is correction factor
double test = q_data[3]*q_data[1] - q_data[2]*q_data[0];
if (test > 0.49999*unit) { // singularity at north pole
_ang << 2 * std::atan2(q_data[0], q_data[3]), M_PI/2, 0;
double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3};
vect3 euler_ang(temp, 3);
return euler_ang;
}
if (test < -0.49999*unit) { // singularity at south pole
_ang << -2 * std::atan2(q_data[0], q_data[3]), -M_PI/2, 0;
double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3};
vect3 euler_ang(temp, 3);
return euler_ang;
}
_ang <<
std::atan2(2*q_data[0]*q_data[3]+2*q_data[1]*q_data[2] , -sqx - sqy + sqz + sqw),
std::asin (2*test/unit),
std::atan2(2*q_data[2]*q_data[3]+2*q_data[1]*q_data[0] , sqx - sqy - sqz + sqw);
double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3};
vect3 euler_ang(temp, 3);
return euler_ang;
}
void h_model_input(state_input &s, esekfom::dyn_share_modified<double> &ekfom_data)
{
bool match_in_map = false;
VF(4) pabcd;
pabcd.setZero();
normvec->resize(time_seq[k]);
int effect_num_k = 0;
for (int j = 0; j < time_seq[k]; j++)
{
PointType &point_body_j = feats_down_body->points[idx+j+1];
PointType &point_world_j = feats_down_world->points[idx+j+1];
pointBodyToWorld(&point_body_j, &point_world_j);
V3D p_body = pbody_list[idx+j+1];
V3D p_world;
p_world << point_world_j.x, point_world_j.y, point_world_j.z;
{
auto &points_near = Nearest_Points[idx+j+1];
ikdtree.Nearest_Search(point_world_j, NUM_MATCH_POINTS, points_near, pointSearchSqDis, 2.236); //1.0); //, 3.0); // 2.236;
if ((points_near.size() < NUM_MATCH_POINTS) || pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5) // 5)
{
point_selected_surf[idx+j+1] = false;
}
else
{
point_selected_surf[idx+j+1] = false;
if (esti_plane(pabcd, points_near, plane_thr)) //(planeValid)
{
float pd2 = pabcd(0) * point_world_j.x + pabcd(1) * point_world_j.y + pabcd(2) * point_world_j.z + pabcd(3);
if (p_body.norm() > match_s * pd2 * pd2)
{
point_selected_surf[idx+j+1] = true;
normvec->points[j].x = pabcd(0);
normvec->points[j].y = pabcd(1);
normvec->points[j].z = pabcd(2);
normvec->points[j].intensity = pabcd(3);
effect_num_k ++;
}
}
}
}
}
if (effect_num_k == 0)
{
ekfom_data.valid = false;
return;
}
ekfom_data.M_Noise = laser_point_cov;
ekfom_data.h_x = Eigen::MatrixXd::Zero(effect_num_k, 12);
ekfom_data.z.resize(effect_num_k);
int m = 0;
for (int j = 0; j < time_seq[k]; j++)
{
if(point_selected_surf[idx+j+1])
{
V3D norm_vec(normvec->points[j].x, normvec->points[j].y, normvec->points[j].z);
if (extrinsic_est_en)
{
V3D p_body = pbody_list[idx+j+1];
M3D p_crossmat, p_imu_crossmat;
p_crossmat << SKEW_SYM_MATRX(p_body);
V3D point_imu = s.offset_R_L_I.normalized() * p_body + s.offset_T_L_I;
p_imu_crossmat << SKEW_SYM_MATRX(point_imu);
V3D C(s.rot.conjugate().normalized() * norm_vec);
V3D A(p_imu_crossmat * C);
V3D B(p_crossmat * s.offset_R_L_I.conjugate().normalized() * C);
ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C);
}
else
{
M3D point_crossmat = crossmat_list[idx+j+1];
V3D C(s.rot.conjugate().normalized() * norm_vec);
V3D A(point_crossmat * C);
ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
}
ekfom_data.z(m) = -norm_vec(0) * feats_down_world->points[idx+j+1].x -norm_vec(1) * feats_down_world->points[idx+j+1].y -norm_vec(2) * feats_down_world->points[idx+j+1].z-normvec->points[j].intensity;
m++;
}
}
effct_feat_num += effect_num_k;
}
void h_model_output(state_output &s, esekfom::dyn_share_modified<double> &ekfom_data)
{
bool match_in_map = false;
VF(4) pabcd;
pabcd.setZero();
normvec->resize(time_seq[k]);
int effect_num_k = 0;
for (int j = 0; j < time_seq[k]; j++)
{
PointType &point_body_j = feats_down_body->points[idx+j+1];
PointType &point_world_j = feats_down_world->points[idx+j+1];
pointBodyToWorld(&point_body_j, &point_world_j);
V3D p_body = pbody_list[idx+j+1];
V3D p_world;
p_world << point_world_j.x, point_world_j.y, point_world_j.z;
{
auto &points_near = Nearest_Points[idx+j+1];
ikdtree.Nearest_Search(point_world_j, NUM_MATCH_POINTS, points_near, pointSearchSqDis, 2.236);
if ((points_near.size() < NUM_MATCH_POINTS) || pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5)
{
point_selected_surf[idx+j+1] = false;
}
else
{
point_selected_surf[idx+j+1] = false;
if (esti_plane(pabcd, points_near, plane_thr)) //(planeValid)
{
float pd2 = pabcd(0) * point_world_j.x + pabcd(1) * point_world_j.y + pabcd(2) * point_world_j.z + pabcd(3);
if (p_body.norm() > match_s * pd2 * pd2)
{
// point_selected_surf[i] = true;
point_selected_surf[idx+j+1] = true;
normvec->points[j].x = pabcd(0);
normvec->points[j].y = pabcd(1);
normvec->points[j].z = pabcd(2);
normvec->points[j].intensity = pabcd(3);
effect_num_k ++;
}
}
}
}
}
if (effect_num_k == 0)
{
ekfom_data.valid = false;
return;
}
ekfom_data.M_Noise = laser_point_cov;
ekfom_data.h_x = Eigen::MatrixXd::Zero(effect_num_k, 12);
ekfom_data.z.resize(effect_num_k);
int m = 0;
for (int j = 0; j < time_seq[k]; j++)
{
if(point_selected_surf[idx+j+1])
{
V3D norm_vec(normvec->points[j].x, normvec->points[j].y, normvec->points[j].z);
if (extrinsic_est_en)
{
V3D p_body = pbody_list[idx+j+1];
M3D p_crossmat, p_imu_crossmat;
p_crossmat << SKEW_SYM_MATRX(p_body);
V3D point_imu = s.offset_R_L_I.normalized() * p_body + s.offset_T_L_I;
p_imu_crossmat << SKEW_SYM_MATRX(point_imu);
V3D C(s.rot.conjugate().normalized() * norm_vec);
V3D A(p_imu_crossmat * C);
V3D B(p_crossmat * s.offset_R_L_I.conjugate().normalized() * C);
ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C);
}
else
{
M3D point_crossmat = crossmat_list[idx+j+1];
V3D C(s.rot.conjugate().normalized() * norm_vec);
V3D A(point_crossmat * C);
// V3D A(point_crossmat * state.rot_end.transpose() * norm_vec);
ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
}
ekfom_data.z(m) = -norm_vec(0) * feats_down_world->points[idx+j+1].x -norm_vec(1) * feats_down_world->points[idx+j+1].y -norm_vec(2) * feats_down_world->points[idx+j+1].z-normvec->points[j].intensity;
m++;
}
}
effct_feat_num += effect_num_k;
}
void h_model_IMU_output(state_output &s, esekfom::dyn_share_modified<double> &ekfom_data)
{
std::memset(ekfom_data.satu_check, false, 6);
ekfom_data.z_IMU.block<3,1>(0, 0) = angvel_avr - s.omg - s.bg;
ekfom_data.z_IMU.block<3,1>(3, 0) = acc_avr * G_m_s2 / acc_norm - s.acc - s.ba;
ekfom_data.R_IMU << imu_meas_omg_cov, imu_meas_omg_cov, imu_meas_omg_cov, imu_meas_acc_cov, imu_meas_acc_cov, imu_meas_acc_cov;
if(check_satu)
{
if(fabs(angvel_avr(0)) >= 0.99 * satu_gyro)
{
ekfom_data.satu_check[0] = true;
ekfom_data.z_IMU(0) = 0.0;
}
if(fabs(angvel_avr(1)) >= 0.99 * satu_gyro)
{
ekfom_data.satu_check[1] = true;
ekfom_data.z_IMU(1) = 0.0;
}
if(fabs(angvel_avr(2)) >= 0.99 * satu_gyro)
{
ekfom_data.satu_check[2] = true;
ekfom_data.z_IMU(2) = 0.0;
}
if(fabs(acc_avr(0)) >= 0.99 * satu_acc)
{
ekfom_data.satu_check[3] = true;
ekfom_data.z_IMU(3) = 0.0;
}
if(fabs(acc_avr(1)) >= 0.99 * satu_acc)
{
ekfom_data.satu_check[4] = true;
ekfom_data.z_IMU(4) = 0.0;
}
if(fabs(acc_avr(2)) >= 0.99 * satu_acc)
{
ekfom_data.satu_check[5] = true;
ekfom_data.z_IMU(5) = 0.0;
}
}
}
void pointBodyToWorld(PointType const * const pi, PointType * const po)
{
V3D p_body(pi->x, pi->y, pi->z);
V3D p_global;
if (extrinsic_est_en)
{
if (!use_imu_as_input)
{
p_global = kf_output.x_.rot.normalized() * (kf_output.x_.offset_R_L_I.normalized() * p_body + kf_output.x_.offset_T_L_I) + kf_output.x_.pos;
}
else
{
p_global = kf_input.x_.rot.normalized() * (kf_input.x_.offset_R_L_I.normalized() * p_body + kf_input.x_.offset_T_L_I) + kf_input.x_.pos;
}
}
else
{
if (!use_imu_as_input)
{
p_global = kf_output.x_.rot.normalized() * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_output.x_.pos;
}
else
{
p_global = kf_input.x_.rot.normalized() * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_input.x_.pos;
}
}
po->x = p_global(0);
po->y = p_global(1);
po->z = p_global(2);
po->intensity = pi->intensity;
}
const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);};
point_lio_ros2/src/Estimator.h
+118
View File
@@ -0,0 +1,118 @@
#ifndef Estimator_H
#define Estimator_H
#include <../include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp>
#include "common_lib.h"
#include "parameters.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <ikd-Tree/ikd_Tree.h>
#include <pcl/io/pcd_io.h>
extern PointCloudXYZI::Ptr normvec; //(new PointCloudXYZI(100000, 1));
extern std::vector<int> time_seq;
extern PointCloudXYZI::Ptr feats_down_body; //(new PointCloudXYZI());
extern PointCloudXYZI::Ptr feats_down_world; //(new PointCloudXYZI());
extern std::vector<V3D> pbody_list;
extern std::vector<PointVector> Nearest_Points;
extern KD_TREE<PointType> ikdtree;
extern std::vector<float> pointSearchSqDis;
extern bool point_selected_surf[100000]; // = {0};
extern std::vector<M3D> crossmat_list;
extern int effct_feat_num;
extern int k;
extern int idx;
extern V3D angvel_avr, acc_avr;
extern V3D Lidar_T_wrt_IMU; //(Zero3d);
extern M3D Lidar_R_wrt_IMU; //(Eye3d);
typedef MTK::vect<3, double> vect3;
typedef MTK::SO3<double> SO3;
typedef MTK::S2<double, 98090, 10000, 1> S2;
typedef MTK::vect<1, double> vect1;
typedef MTK::vect<2, double> vect2;
MTK_BUILD_MANIFOLD(state_input,
((vect3, pos))
((SO3, rot))
((SO3, offset_R_L_I))
((vect3, offset_T_L_I))
((vect3, vel))
((vect3, bg))
((vect3, ba))
((vect3, gravity))
);
MTK_BUILD_MANIFOLD(state_output,
((vect3, pos))
((SO3, rot))
((SO3, offset_R_L_I))
((vect3, offset_T_L_I))
((vect3, vel))
((vect3, omg))
((vect3, acc))
((vect3, gravity))
((vect3, bg))
((vect3, ba))
);
MTK_BUILD_MANIFOLD(input_ikfom,
((vect3, acc))
((vect3, gyro))
);
MTK_BUILD_MANIFOLD(process_noise_input,
((vect3, ng))
((vect3, na))
((vect3, nbg))
((vect3, nba))
);
MTK_BUILD_MANIFOLD(process_noise_output,
((vect3, vel))
((vect3, ng))
((vect3, na))
((vect3, nbg))
((vect3, nba))
);
extern esekfom::esekf<state_input, 24, input_ikfom> kf_input;
extern esekfom::esekf<state_output, 30, input_ikfom> kf_output;
extern state_input state_in;
extern state_output state_out;
extern input_ikfom input_in;
Eigen::Matrix<double, 24, 24> process_noise_cov_input();
Eigen::Matrix<double, 30, 30> process_noise_cov_output();
//double L_offset_to_I[3] = {0.04165, 0.02326, -0.0284}; // Avia
//vect3 Lidar_offset_to_IMU(L_offset_to_I, 3);
Eigen::Matrix<double, 24, 1> get_f_input(state_input &s, const input_ikfom &in);
Eigen::Matrix<double, 30, 1> get_f_output(state_output &s, const input_ikfom &in);
Eigen::Matrix<double, 24, 24> df_dx_input(state_input &s, const input_ikfom &in);
// Eigen::Matrix<double, 24, 12> df_dw_input(state_input &s, const input_ikfom &in);
Eigen::Matrix<double, 30, 30> df_dx_output(state_output &s, const input_ikfom &in);
// Eigen::Matrix<double, 30, 15> df_dw_output(state_output &s);
vect3 SO3ToEuler(const SO3 &orient);
void h_model_input(state_input &s, esekfom::dyn_share_modified<double> &ekfom_data);
void h_model_output(state_output &s, esekfom::dyn_share_modified<double> &ekfom_data);
void h_model_IMU_output(state_output &s, esekfom::dyn_share_modified<double> &ekfom_data);
void pointBodyToWorld(PointType const *const pi, PointType *const po);
const bool time_list(PointType &x, PointType &y); // {return (x.curvature < y.curvature);};
#endif
point_lio_ros2/src/IMU_Processing.hpp
+164
View File
@@ -0,0 +1,164 @@
#include <cmath>
#include <math.h>
#include <deque>
#include <mutex>
#include <thread>
#include <fstream>
#include <csignal>
#include <rclcpp/rclcpp.hpp>
#include <so3_math.h>
#include <Eigen/Eigen>
#include <common_lib.h>
#include <pcl/common/io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <condition_variable>
#include <nav_msgs/msg/odometry.hpp>
#include <pcl/common/transforms.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <tf2_ros/transform_broadcaster.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <geometry_msgs/msg/vector3.hpp>
/// *************Preconfiguration
#define MAX_INI_COUNT (100)
/// *************IMU Process and undistortion
class ImuProcess {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ImuProcess();
~ImuProcess();
void Reset();
//void Reset(double start_timestamp, const sensor_msgs::msg::Imu::ConstSharedPtr &lastimu);
void Process(const MeasureGroup &meas, const PointCloudXYZI::Ptr &pcl_un_);
void Set_init(Eigen::Vector3d &tmp_gravity, Eigen::Matrix3d &rot);
ofstream fout_imu;
// double first_lidar_time;
int lidar_type;
bool imu_en;
V3D mean_acc, gravity_;
bool imu_need_init_ = true;
bool b_first_frame_ = true;
bool gravity_align_ = false;
private:
void IMU_init(const MeasureGroup &meas, int &N);
V3D mean_gyr;
int init_iter_num = 1;
rclcpp::Logger logger;
};
ImuProcess::ImuProcess()
: b_first_frame_(true), imu_need_init_(true), gravity_align_(false),
logger(rclcpp::get_logger("laserMapping")) {
imu_en = true;
init_iter_num = 1;
mean_acc = V3D(0, 0, -1.0);
mean_gyr = V3D(0, 0, 0);
}
ImuProcess::~ImuProcess() {}
void ImuProcess::Reset() {
RCLCPP_WARN(logger, "Reset ImuProcess");
mean_acc = V3D(0, 0, -1.0);
mean_gyr = V3D(0, 0, 0);
imu_need_init_ = true;
init_iter_num = 1;
}
void ImuProcess::IMU_init(const MeasureGroup &meas, int &N) {
/** 1. initializing the gravity, gyro bias, acc and gyro covariance
** 2. normalize the acceleration measurenments to unit gravity **/
RCLCPP_INFO(logger, "IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100);
V3D cur_acc, cur_gyr;
if (b_first_frame_) {
Reset();
N = 1;
b_first_frame_ = false;
const auto &imu_acc = meas.imu.front()->linear_acceleration;
const auto &gyr_acc = meas.imu.front()->angular_velocity;
mean_acc << imu_acc.x, imu_acc.y, imu_acc.z;
mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
}
for (const auto &imu: meas.imu) {
const auto &imu_acc = imu->linear_acceleration;
const auto &gyr_acc = imu->angular_velocity;
cur_acc << imu_acc.x, imu_acc.y, imu_acc.z;
cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
mean_acc += (cur_acc - mean_acc) / N;
mean_gyr += (cur_gyr - mean_gyr) / N;
N++;
}
}
void ImuProcess::Process(const MeasureGroup &meas, const PointCloudXYZI::Ptr &cur_pcl_un_) {
if (imu_en) {
if (meas.imu.empty()) return;
assert(meas.lidar != nullptr);
if (imu_need_init_) {
/// The very first lidar frame
IMU_init(meas, init_iter_num);
imu_need_init_ = true;
if (init_iter_num > MAX_INI_COUNT) {
RCLCPP_INFO(logger, "IMU Initializing: %.1f %%", 100.0);
imu_need_init_ = false;
*cur_pcl_un_ = *(meas.lidar);
}
return;
}
if (!gravity_align_) gravity_align_ = true;
*cur_pcl_un_ = *(meas.lidar);
return;
} else {
if (!b_first_frame_) { if (!gravity_align_) gravity_align_ = true; }
else {
b_first_frame_ = false;
return;
}
*cur_pcl_un_ = *(meas.lidar);
return;
}
}
void ImuProcess::Set_init(Eigen::Vector3d &tmp_gravity, Eigen::Matrix3d &rot) {
/** 1. initializing the gravity, gyro bias, acc and gyro covariance
** 2. normalize the acceleration measurenments to unit gravity **/
// V3D tmp_gravity = - mean_acc / mean_acc.norm() * G_m_s2; // state_gravity;
M3D hat_grav;
hat_grav << 0.0, gravity_(2), -gravity_(1),
-gravity_(2), 0.0, gravity_(0),
gravity_(1), -gravity_(0), 0.0;
double align_norm = (hat_grav * tmp_gravity).norm() / tmp_gravity.norm() / gravity_.norm();
double align_cos = gravity_.transpose() * tmp_gravity;
align_cos = align_cos / gravity_.norm() / tmp_gravity.norm();
if (align_norm < 1e-6) {
if (align_cos > 1e-6) {
rot = Eye3d;
} else {
rot = -Eye3d;
}
} else {
V3D align_angle = hat_grav * tmp_gravity / (hat_grav * tmp_gravity).norm() * acos(align_cos);
rot = Exp(align_angle(0), align_angle(1), align_angle(2));
}
}
point_lio_ros2/src/laserMapping.cpp
+1322
View File
File diff suppressed because it is too large Load Diff
point_lio_ros2/src/parameters.cpp
+157
View File
@@ -0,0 +1,157 @@
#include "parameters.h"
bool odom_only;
std::string odom_header_frame_id, odom_child_frame_id;
bool is_first_frame = true;
double lidar_end_time = 0.0, first_lidar_time = 0.0, time_con = 0.0;
double last_timestamp_lidar = -1.0, last_timestamp_imu = -1.0;
int pcd_index = 0;
std::string lid_topic, imu_topic;
bool prop_at_freq_of_imu, check_satu, con_frame, cut_frame;
bool use_imu_as_input, space_down_sample, publish_odometry_without_downsample;
int init_map_size, con_frame_num;
double match_s, satu_acc, satu_gyro, cut_frame_time_interval;
float plane_thr;
double filter_size_surf_min, filter_size_map_min, fov_deg;
double cube_len;
float DET_RANGE;
bool imu_en, gravity_align, non_station_start;
double imu_time_inte;
double laser_point_cov, acc_norm;
double vel_cov, acc_cov_input, gyr_cov_input;
double gyr_cov_output, acc_cov_output, b_gyr_cov, b_acc_cov;
double imu_meas_acc_cov, imu_meas_omg_cov;
int lidar_type, pcd_save_interval;
std::vector<double> gravity_init, gravity;
std::vector<double> extrinT;
std::vector<double> extrinR;
bool runtime_pos_log, pcd_save_en, path_en, extrinsic_est_en = true;
bool scan_pub_en, scan_body_pub_en;
shared_ptr<Preprocess> p_pre;
double time_lag_imu_to_lidar = 0.0;
void readParameters(shared_ptr<rclcpp::Node> &nh) {
p_pre.reset(new Preprocess());
nh->declare_parameter<bool>("odom_only", false);
nh->declare_parameter<std::string>("odom_header_frame_id", "camera_init");
nh->declare_parameter<std::string>("odom_child_frame_id", "aft_mapped");
nh->declare_parameter<bool>("prop_at_freq_of_imu", true);
nh->declare_parameter<bool>("use_imu_as_input", true);
nh->declare_parameter<bool>("check_satu", true);
nh->declare_parameter<int>("init_map_size", 100);
nh->declare_parameter<bool>("space_down_sample", true);
nh->declare_parameter<double>("mapping.satu_acc", 3.0);
nh->declare_parameter<double>("mapping.satu_gyro", 35.0);
nh->declare_parameter<double>("mapping.acc_norm", 1.0);
nh->declare_parameter<float>("mapping.plane_thr", 0.05f);
nh->declare_parameter<int>("point_filter_num", 2);
nh->declare_parameter<std::string>("common.lid_topic", "/livox/lidar");
nh->declare_parameter<std::string>("common.imu_topic", "/livox/imu");
nh->declare_parameter<bool>("common.con_frame", false);
nh->declare_parameter<int>("common.con_frame_num", 1);
nh->declare_parameter<bool>("common.cut_frame", false);
nh->declare_parameter<double>("common.cut_frame_time_interval", 0.1);
nh->declare_parameter<double>("common.time_lag_imu_to_lidar", 0.0);
nh->declare_parameter<double>("filter_size_surf", 0.5);
nh->declare_parameter<double>("filter_size_map", 0.5);
nh->declare_parameter<double>("cube_side_length", 200);
nh->declare_parameter<float>("mapping.det_range", 300.f);
nh->declare_parameter<double>("mapping.fov_degree", 180);
nh->declare_parameter<bool>("mapping.imu_en", true);
nh->declare_parameter<bool>("mapping.start_in_aggressive_motion", false);
nh->declare_parameter<bool>("mapping.extrinsic_est_en", true);
nh->declare_parameter<double>("mapping.imu_time_inte", 0.005);
nh->declare_parameter<double>("mapping.lidar_meas_cov", 0.1);
nh->declare_parameter<double>("mapping.acc_cov_input", 0.1);
nh->declare_parameter<double>("mapping.vel_cov", 20);
nh->declare_parameter<double>("mapping.gyr_cov_input", 0.1);
nh->declare_parameter<double>("mapping.gyr_cov_output", 0.1);
nh->declare_parameter<double>("mapping.acc_cov_output", 0.1);
nh->declare_parameter<double>("mapping.b_gyr_cov", 0.0001);
nh->declare_parameter<double>("mapping.b_acc_cov", 0.0001);
nh->declare_parameter<double>("mapping.imu_meas_acc_cov", 0.1);
nh->declare_parameter<double>("mapping.imu_meas_omg_cov", 0.1);
nh->declare_parameter<double>("preprocess.blind", 1.0);
nh->declare_parameter<int>("preprocess.lidar_type", 1);
nh->declare_parameter<int>("preprocess.scan_line", 16);
nh->declare_parameter<int>("preprocess.scan_rate", 10);
nh->declare_parameter<int>("preprocess.timestamp_unit", 1);
nh->declare_parameter<double>("mapping.match_s", 81);
nh->declare_parameter<bool>("mapping.gravity_align", true);
nh->declare_parameter<std::vector<double>>("mapping.gravity", {0, 0, -9.810});
nh->declare_parameter<std::vector<double>>("mapping.gravity_init", {0, 0, -9.810});
nh->declare_parameter<std::vector<double>>("mapping.extrinsic_T", {0, 0, 0});
nh->declare_parameter<std::vector<double>>("mapping.extrinsic_R", {1, 0, 0, 0, 1, 0, 0, 0, 1});
nh->declare_parameter<bool>("odometry.publish_odometry_without_downsample", false);
nh->declare_parameter<bool>("publish.path_en", true);
nh->declare_parameter<bool>("publish.scan_publish_en", true);
nh->declare_parameter<bool>("publish.scan_bodyframe_pub_en", true);
nh->declare_parameter<bool>("runtime_pos_log_enable", false);
nh->declare_parameter<bool>("pcd_save.pcd_save_en", false);
nh->declare_parameter<int>("pcd_save.interval", -1);
// 使用get_parameter方法获取参数值
nh->get_parameter("odom_only", odom_only);
nh->get_parameter("odom_header_frame_id", odom_header_frame_id);
nh->get_parameter("odom_child_frame_id", odom_child_frame_id);
nh->get_parameter("prop_at_freq_of_imu", prop_at_freq_of_imu);
nh->get_parameter("use_imu_as_input", use_imu_as_input);
nh->get_parameter("check_satu", check_satu);
nh->get_parameter("init_map_size", init_map_size);
nh->get_parameter("space_down_sample", space_down_sample);
nh->get_parameter("mapping.satu_acc", satu_acc);
nh->get_parameter("mapping.satu_gyro", satu_gyro);
nh->get_parameter("mapping.acc_norm", acc_norm);
nh->get_parameter("mapping.plane_thr", plane_thr);
nh->get_parameter("point_filter_num", p_pre->point_filter_num);
nh->get_parameter("common.lid_topic", lid_topic);
nh->get_parameter("common.imu_topic", imu_topic);
nh->get_parameter("common.con_frame", con_frame);
nh->get_parameter("common.con_frame_num", con_frame_num);
nh->get_parameter("common.cut_frame", cut_frame);
nh->get_parameter("common.cut_frame_time_interval", cut_frame_time_interval);
nh->get_parameter("common.time_lag_imu_to_lidar", time_lag_imu_to_lidar);
nh->get_parameter("filter_size_surf", filter_size_surf_min);
nh->get_parameter("filter_size_map", filter_size_map_min);
nh->get_parameter("cube_side_length", cube_len);
nh->get_parameter("mapping.det_range", DET_RANGE);
nh->get_parameter("mapping.fov_degree", fov_deg);
nh->get_parameter("mapping.imu_en", imu_en);
nh->get_parameter("mapping.start_in_aggressive_motion", non_station_start);
nh->get_parameter("mapping.extrinsic_est_en", extrinsic_est_en);
nh->get_parameter("mapping.imu_time_inte", imu_time_inte);
nh->get_parameter("mapping.lidar_meas_cov", laser_point_cov);
nh->get_parameter("mapping.acc_cov_input", acc_cov_input);
nh->get_parameter("mapping.vel_cov", vel_cov);
nh->get_parameter("mapping.gyr_cov_input", gyr_cov_input);
nh->get_parameter("mapping.gyr_cov_output", gyr_cov_output);
nh->get_parameter("mapping.acc_cov_output", acc_cov_output);
nh->get_parameter("mapping.b_gyr_cov", b_gyr_cov);
nh->get_parameter("mapping.b_acc_cov", b_acc_cov);
nh->get_parameter("mapping.imu_meas_acc_cov", imu_meas_acc_cov);
nh->get_parameter("mapping.imu_meas_omg_cov", imu_meas_omg_cov);
nh->get_parameter("preprocess.blind", p_pre->blind);
nh->get_parameter("preprocess.lidar_type", lidar_type);
nh->get_parameter("preprocess.scan_line", p_pre->N_SCANS);
nh->get_parameter("preprocess.scan_rate", p_pre->SCAN_RATE);
nh->get_parameter("preprocess.timestamp_unit", p_pre->time_unit);
nh->get_parameter("mapping.match_s", match_s);
nh->get_parameter("mapping.gravity_align", gravity_align);
nh->get_parameter("mapping.gravity", gravity);
nh->get_parameter("mapping.gravity_init", gravity_init);
nh->get_parameter("mapping.extrinsic_T", extrinT);
nh->get_parameter("mapping.extrinsic_R", extrinR);
nh->get_parameter("odometry.publish_odometry_without_downsample", publish_odometry_without_downsample);
nh->get_parameter("publish.path_en", path_en);
nh->get_parameter("publish.scan_publish_en", scan_pub_en);
nh->get_parameter("publish.scan_bodyframe_pub_en", scan_body_pub_en);
nh->get_parameter("runtime_pos_log_enable", runtime_pos_log);
nh->get_parameter("pcd_save.pcd_save_en", pcd_save_en);
nh->get_parameter("pcd_save.interval", pcd_save_interval);
}
point_lio_ros2/src/parameters.h
+46
View File
@@ -0,0 +1,46 @@
// #ifndef PARAM_H
// #define PARAM_H
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <Eigen/Eigen>
#include <Eigen/Core>
#include <cstring>
#include <string>
#include "preprocess.h"
extern bool odom_only;
extern std::string odom_header_frame_id;
extern std::string odom_child_frame_id;
extern bool is_first_frame;
extern double lidar_end_time, first_lidar_time, time_con;
extern double last_timestamp_lidar, last_timestamp_imu;
extern int pcd_index;
extern std::string lid_topic, imu_topic;
extern bool prop_at_freq_of_imu, check_satu, con_frame, cut_frame;
extern bool use_imu_as_input, space_down_sample;
extern bool extrinsic_est_en, publish_odometry_without_downsample;
extern int init_map_size, con_frame_num;
extern double match_s, satu_acc, satu_gyro, cut_frame_time_interval;
extern float plane_thr;
extern double filter_size_surf_min, filter_size_map_min, fov_deg;
extern double cube_len;
extern float DET_RANGE;
extern bool imu_en, gravity_align, non_station_start;
extern double imu_time_inte;
extern double laser_point_cov, acc_norm;
extern double acc_cov_input, gyr_cov_input, vel_cov;
extern double gyr_cov_output, acc_cov_output, b_gyr_cov, b_acc_cov;
extern double imu_meas_acc_cov, imu_meas_omg_cov;
extern int lidar_type, pcd_save_interval;
extern std::vector<double> gravity_init, gravity;
extern std::vector<double> extrinT;
extern std::vector<double> extrinR;
extern bool runtime_pos_log, pcd_save_en, path_en;
extern bool scan_pub_en, scan_body_pub_en;
extern shared_ptr<Preprocess> p_pre;
extern double time_lag_imu_to_lidar;
void readParameters(shared_ptr<rclcpp::Node> &nh);
point_lio_ros2/src/preprocess.cpp
+732
View File
@@ -0,0 +1,732 @@
#include "preprocess.h"
#define RETURN0 0x00
#define RETURN0AND1 0x10
Preprocess::Preprocess()
: lidar_type(AVIA), blind(0.01), point_filter_num(1) {
inf_bound = 10;
N_SCANS = 6;
SCAN_RATE = 10;
group_size = 8;
disA = 0.01;
disA = 0.1; // B?
p2l_ratio = 225;
limit_maxmid = 6.25;
limit_midmin = 6.25;
limit_maxmin = 3.24;
jump_up_limit = 170.0;
jump_down_limit = 8.0;
cos160 = 160.0;
edgea = 2;
edgeb = 0.1;
smallp_intersect = 172.5;
smallp_ratio = 1.2;
given_offset_time = false;
jump_up_limit = cos(jump_up_limit / 180 * M_PI);
jump_down_limit = cos(jump_down_limit / 180 * M_PI);
cos160 = cos(cos160 / 180 * M_PI);
smallp_intersect = cos(smallp_intersect / 180 * M_PI);
}
Preprocess::~Preprocess() {}
void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num) {
lidar_type = lid_type;
blind = bld;
point_filter_num = pfilt_num;
}
// void Preprocess::process(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out) {
// avia_handler(msg);
// *pcl_out = pl_surf;
// }
void Preprocess::process(const sensor_msgs::msg::PointCloud2::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out) {
switch (time_unit) {
case SEC:
time_unit_scale = 1.e3f;
break;
case MS:
time_unit_scale = 1.f;
break;
case US:
time_unit_scale = 1.e-3f;
break;
case NS:
time_unit_scale = 1.e-6f;
break;
default:
time_unit_scale = 1.f;
break;
}
switch (lidar_type) {
case OUST64:
oust64_handler(msg);
break;
case VELO16:
velodyne_handler(msg);
break;
case HESAIxt32:
hesai_handler(msg);
break;
case UNILIDAR:
unilidar_handler(msg);
break;
default:
printf("Error LiDAR Type");
break;
}
*pcl_out = pl_surf;
}
// void Preprocess::avia_handler(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg) {
// pl_surf.clear();
// pl_corn.clear();
// pl_full.clear();
// double t1 = omp_get_wtime();
// int plsize = msg->point_num;
// pl_corn.reserve(plsize);
// pl_surf.reserve(plsize);
// pl_full.resize(plsize);
// uint valid_num = 0;
// for (uint i = 1; i < plsize; i++) {
// if ((msg->points[i].line < N_SCANS) &&
// ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) {
// valid_num++;
// if (valid_num % point_filter_num == 0) {
// pl_full[i].x = msg->points[i].x;
// pl_full[i].y = msg->points[i].y;
// pl_full[i].z = msg->points[i].z;
// pl_full[i].intensity = msg->points[i].reflectivity;
// pl_full[i].curvature = msg->points[i].offset_time /
// float(1000000); // use curvature as time of each laser points, curvature unit: ms
// if (i == 0) pl_full[i].curvature = fabs(pl_full[i].curvature) < 1.0 ? pl_full[i].curvature : 0.0;
// else pl_full[i].curvature =
// fabs(pl_full[i].curvature - pl_full[i - 1].curvature) < 1.0 ? pl_full[i].curvature :
// pl_full[i - 1].curvature + 0.004166667f;
// if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7)
// || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7)
// || (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7)
// && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z >
// (blind * blind))) {
// pl_surf.push_back(pl_full[i]);
// }
// }
// }
// }
// }
void Preprocess::oust64_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg) {
pl_surf.clear();
pl_corn.clear();
pl_full.clear();
pcl::PointCloud<ouster_ros::Point> pl_orig;
pcl::fromROSMsg(*msg, pl_orig);
int plsize = pl_orig.size();
pl_corn.reserve(plsize);
pl_surf.reserve(plsize);
double time_stamp = rclcpp::Time(msg->header.stamp).seconds();
// cout << "===================================" << endl;
// printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS);
for (int i = 0; i < pl_orig.points.size(); i++) {
if (i % point_filter_num != 0) continue;
double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y +
pl_orig.points[i].z * pl_orig.points[i].z;
if (range < (blind * blind)) continue;
Eigen::Vector3d pt_vec;
PointType added_pt;
added_pt.x = pl_orig.points[i].x;
added_pt.y = pl_orig.points[i].y;
added_pt.z = pl_orig.points[i].z;
added_pt.intensity = pl_orig.points[i].intensity;
added_pt.normal_x = 0;
added_pt.normal_y = 0;
added_pt.normal_z = 0;
added_pt.curvature = pl_orig.points[i].t * time_unit_scale; // curvature unit: ms
pl_surf.points.push_back(added_pt);
}
// pub_func(pl_surf, pub_full, msg->header.stamp);
// pub_func(pl_surf, pub_corn, msg->header.stamp);
}
void Preprocess::velodyne_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg) {
pl_surf.clear();
pl_corn.clear();
pl_full.clear();
pcl::PointCloud<velodyne_ros::Point> pl_orig;
pcl::fromROSMsg(*msg, pl_orig);
int plsize = pl_orig.points.size();
if (plsize == 0) return;
pl_surf.reserve(plsize);
/*** These variables only works when no point timestamps given ***/
double omega_l = 0.361 * SCAN_RATE; // scan angular velocity
std::vector<bool> is_first(N_SCANS, true);
std::vector<double> yaw_fp(N_SCANS, 0.0); // yaw of first scan point
std::vector<float> yaw_last(N_SCANS, 0.0); // yaw of last scan point
std::vector<float> time_last(N_SCANS, 0.0); // last offset time
/*****************************************************************/
if (pl_orig.points[plsize - 1].time > 0) {
given_offset_time = true;
} else {
given_offset_time = false;
double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;
double yaw_end = yaw_first;
int layer_first = pl_orig.points[0].ring;
for (uint i = plsize - 1; i > 0; i--) {
if (pl_orig.points[i].ring == layer_first) {
yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;
break;
}
}
}
for (int i = 0; i < plsize; i++) {
PointType added_pt;
// cout<<"!!!!!!"<<i<<" "<<plsize<<endl;
added_pt.normal_x = 0;
added_pt.normal_y = 0;
added_pt.normal_z = 0;
added_pt.x = pl_orig.points[i].x;
added_pt.y = pl_orig.points[i].y;
added_pt.z = pl_orig.points[i].z;
added_pt.intensity = pl_orig.points[i].intensity;
added_pt.curvature = pl_orig.points[i].time * time_unit_scale; // curvature unit: ms // cout<<added_pt.curvature<<endl;
if (!given_offset_time) {
int layer = pl_orig.points[i].ring;
double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
if (is_first[layer]) {
// printf("layer: %d; is first: %d", layer, is_first[layer]);
yaw_fp[layer] = yaw_angle;
is_first[layer] = false;
added_pt.curvature = 0.0;
yaw_last[layer] = yaw_angle;
time_last[layer] = added_pt.curvature;
continue;
}
// compute offset time
if (yaw_angle <= yaw_fp[layer]) {
added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l;
} else {
added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l;
}
if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l;
yaw_last[layer] = yaw_angle;
time_last[layer] = added_pt.curvature;
}
if (i % point_filter_num == 0) {
if (added_pt.x * added_pt.x
+ added_pt.y * added_pt.y
+ added_pt.z * added_pt.z > (blind * blind))
{
pl_surf.points.push_back(added_pt);
}
}
}
}
void Preprocess::unilidar_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg)
{
pl_surf.clear();
pl_corn.clear();
pl_full.clear();
pcl::PointCloud<unilidar_ros::Point> pl_orig;
pcl::fromROSMsg(*msg, pl_orig);
int plsize = pl_orig.points.size();
if (plsize == 0) return;
pl_surf.reserve(plsize);
// std::cout << "plsize = " << plsize << ", given_offset_time = " << given_offset_time << std::endl;
int countElimnated = 0;
for (int i = 0; i < plsize; i++)
{
PointType added_pt;
added_pt.normal_x = 0;
added_pt.normal_y = 0;
added_pt.normal_z = 0;
added_pt.x = pl_orig.points[i].x;
added_pt.y = pl_orig.points[i].y;
added_pt.z = pl_orig.points[i].z;
added_pt.intensity = pl_orig.points[i].intensity;
added_pt.curvature = pl_orig.points[i].time * time_unit_scale;
if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > (blind * blind))
{
pl_surf.points.push_back(added_pt);
}
else
{
countElimnated++;
}
}
// std::cout << "pl_surf.size() = " << pl_surf.size() << ", countElimnated = " << countElimnated << std::endl;
}
void Preprocess::hesai_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg) {
pl_surf.clear();
pl_corn.clear();
pl_full.clear();
pcl::PointCloud<hesai_ros::Point> pl_orig;
pcl::fromROSMsg(*msg, pl_orig);
int plsize = pl_orig.points.size();
if (plsize == 0) return;
pl_surf.reserve(plsize);
/*** These variables only works when no point timestamps given ***/
double omega_l = 0.361 * SCAN_RATE; // scan angular velocity
std::vector<bool> is_first(N_SCANS, true);
std::vector<double> yaw_fp(N_SCANS, 0.0); // yaw of first scan point
std::vector<float> yaw_last(N_SCANS, 0.0); // yaw of last scan point
std::vector<float> time_last(N_SCANS, 0.0); // last offset time
/*****************************************************************/
if (pl_orig.points[plsize - 1].timestamp > 0) {
given_offset_time = true;
} else {
given_offset_time = false;
double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;
double yaw_end = yaw_first;
int layer_first = pl_orig.points[0].ring;
for (uint i = plsize - 1; i > 0; i--) {
if (pl_orig.points[i].ring == layer_first) {
yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;
break;
}
}
}
double time_head = pl_orig.points[0].timestamp;
for (int i = 0; i < plsize; i++) {
PointType added_pt;
// cout<<"!!!!!!"<<i<<" "<<plsize<<endl;
added_pt.normal_x = 0;
added_pt.normal_y = 0;
added_pt.normal_z = 0;
added_pt.x = pl_orig.points[i].x;
added_pt.y = pl_orig.points[i].y;
added_pt.z = pl_orig.points[i].z;
added_pt.intensity = pl_orig.points[i].intensity;
added_pt.curvature = (pl_orig.points[i].timestamp - time_head) *
1000.f; // time_unit_scale; // curvature unit: ms // cout<<added_pt.curvature<<endl;
if (!given_offset_time) {
int layer = pl_orig.points[i].ring;
double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
if (is_first[layer]) {
// printf("layer: %d; is first: %d", layer, is_first[layer]);
yaw_fp[layer] = yaw_angle;
is_first[layer] = false;
added_pt.curvature = 0.0;
yaw_last[layer] = yaw_angle;
time_last[layer] = added_pt.curvature;
continue;
}
// compute offset time
if (yaw_angle <= yaw_fp[layer]) {
added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l;
} else {
added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l;
}
if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l;
yaw_last[layer] = yaw_angle;
time_last[layer] = added_pt.curvature;
}
if (i % point_filter_num == 0) {
if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > (blind * blind)) {
pl_surf.points.push_back(added_pt);
}
}
}
}
void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types) {
int plsize = pl.size();
int plsize2;
if (plsize == 0) {
printf("something wrong\n");
return;
}
uint head = 0;
while (types[head].range < blind) {
head++;
}
// Surf
plsize2 = (plsize > group_size) ? (plsize - group_size) : 0;
Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero());
Eigen::Vector3d last_direct(Eigen::Vector3d::Zero());
uint i_nex = 0, i2;
uint last_i = 0;
uint last_i_nex = 0;
int last_state = 0;
int plane_type;
for (uint i = head; i < plsize2; i++) {
if (types[i].range < blind) {
continue;
}
i2 = i;
plane_type = plane_judge(pl, types, i, i_nex, curr_direct);
if (plane_type == 1) {
for (uint j = i; j <= i_nex; j++) {
if (j != i && j != i_nex) {
types[j].ftype = Real_Plane;
} else {
types[j].ftype = Poss_Plane;
}
}
// if(last_state==1 && fabs(last_direct.sum())>0.5)
if (last_state == 1 && last_direct.norm() > 0.1) {
double mod = last_direct.transpose() * curr_direct;
if (mod > -0.707 && mod < 0.707) {
types[i].ftype = Edge_Plane;
} else {
types[i].ftype = Real_Plane;
}
}
i = i_nex - 1;
last_state = 1;
} else // if(plane_type == 2)
{
i = i_nex;
last_state = 0;
}
last_i = i2;
last_i_nex = i_nex;
last_direct = curr_direct;
}
plsize2 = plsize > 3 ? plsize - 3 : 0;
for (uint i = head + 3; i < plsize2; i++) {
if (types[i].range < blind || types[i].ftype >= Real_Plane) {
continue;
}
if (types[i - 1].dista < 1e-16 || types[i].dista < 1e-16) {
continue;
}
Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z);
Eigen::Vector3d vecs[2];
for (int j = 0; j < 2; j++) {
int m = -1;
if (j == 1) {
m = 1;
}
if (types[i + m].range < blind) {
if (types[i].range > inf_bound) {
types[i].edj[j] = Nr_inf;
} else {
types[i].edj[j] = Nr_blind;
}
continue;
}
vecs[j] = Eigen::Vector3d(pl[i + m].x, pl[i + m].y, pl[i + m].z);
vecs[j] = vecs[j] - vec_a;
types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm();
if (types[i].angle[j] < jump_up_limit) {
types[i].edj[j] = Nr_180;
} else if (types[i].angle[j] > jump_down_limit) {
types[i].edj[j] = Nr_zero;
}
}
types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm();
if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_zero && types[i].dista > 0.0225 &&
types[i].dista > 4 * types[i - 1].dista) {
if (types[i].intersect > cos160) {
if (edge_jump_judge(pl, types, i, Prev)) {
types[i].ftype = Edge_Jump;
}
}
} else if (types[i].edj[Prev] == Nr_zero && types[i].edj[Next] == Nr_nor && types[i - 1].dista > 0.0225 &&
types[i - 1].dista > 4 * types[i].dista) {
if (types[i].intersect > cos160) {
if (edge_jump_judge(pl, types, i, Next)) {
types[i].ftype = Edge_Jump;
}
}
} else if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_inf) {
if (edge_jump_judge(pl, types, i, Prev)) {
types[i].ftype = Edge_Jump;
}
} else if (types[i].edj[Prev] == Nr_inf && types[i].edj[Next] == Nr_nor) {
if (edge_jump_judge(pl, types, i, Next)) {
types[i].ftype = Edge_Jump;
}
} else if (types[i].edj[Prev] > Nr_nor && types[i].edj[Next] > Nr_nor) {
if (types[i].ftype == Nor) {
types[i].ftype = Wire;
}
}
}
plsize2 = plsize - 1;
double ratio;
for (uint i = head + 1; i < plsize2; i++) {
if (types[i].range < blind || types[i - 1].range < blind || types[i + 1].range < blind) {
continue;
}
if (types[i - 1].dista < 1e-8 || types[i].dista < 1e-8) {
continue;
}
if (types[i].ftype == Nor) {
if (types[i - 1].dista > types[i].dista) {
ratio = types[i - 1].dista / types[i].dista;
} else {
ratio = types[i].dista / types[i - 1].dista;
}
if (types[i].intersect < smallp_intersect && ratio < smallp_ratio) {
if (types[i - 1].ftype == Nor) {
types[i - 1].ftype = Real_Plane;
}
if (types[i + 1].ftype == Nor) {
types[i + 1].ftype = Real_Plane;
}
types[i].ftype = Real_Plane;
}
}
}
int last_surface = -1;
for (uint j = head; j < plsize; j++) {
if (types[j].ftype == Poss_Plane || types[j].ftype == Real_Plane) {
if (last_surface == -1) {
last_surface = j;
}
if (j == uint(last_surface + point_filter_num - 1)) {
PointType ap;
ap.x = pl[j].x;
ap.y = pl[j].y;
ap.z = pl[j].z;
ap.intensity = pl[j].intensity;
ap.curvature = pl[j].curvature;
pl_surf.push_back(ap);
last_surface = -1;
}
} else {
if (types[j].ftype == Edge_Jump || types[j].ftype == Edge_Plane) {
pl_corn.push_back(pl[j]);
}
if (last_surface != -1) {
PointType ap;
for (uint k = last_surface; k < j; k++) {
ap.x += pl[k].x;
ap.y += pl[k].y;
ap.z += pl[k].z;
ap.intensity += pl[k].intensity;
ap.curvature += pl[k].curvature;
}
ap.x /= (j - last_surface);
ap.y /= (j - last_surface);
ap.z /= (j - last_surface);
ap.intensity /= (j - last_surface);
ap.curvature /= (j - last_surface);
pl_surf.push_back(ap);
}
last_surface = -1;
}
}
}
void Preprocess::pub_func(PointCloudXYZI &pl, const rclcpp::Time &ct) {
pl.height = 1;
pl.width = pl.size();
sensor_msgs::msg::PointCloud2 output;
pcl::toROSMsg(pl, output);
output.header.frame_id = "livox";
output.header.stamp = ct;
}
int Preprocess::plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex,
Eigen::Vector3d &curr_direct) {
double group_dis = disA * types[i_cur].range + disB;
group_dis = group_dis * group_dis;
// i_nex = i_cur;
double two_dis;
vector<double> disarr;
disarr.reserve(20);
for (i_nex = i_cur; i_nex < i_cur + group_size; i_nex++) {
if (types[i_nex].range < blind) {
curr_direct.setZero();
return 2;
}
disarr.push_back(types[i_nex].dista);
}
for (;;) {
if ((i_cur >= pl.size()) || (i_nex >= pl.size())) break;
if (types[i_nex].range < blind) {
curr_direct.setZero();
return 2;
}
vx = pl[i_nex].x - pl[i_cur].x;
vy = pl[i_nex].y - pl[i_cur].y;
vz = pl[i_nex].z - pl[i_cur].z;
two_dis = vx * vx + vy * vy + vz * vz;
if (two_dis >= group_dis) {
break;
}
disarr.push_back(types[i_nex].dista);
i_nex++;
}
double leng_wid = 0;
double v1[3], v2[3];
for (uint j = i_cur + 1; j < i_nex; j++) {
if ((j >= pl.size()) || (i_cur >= pl.size())) break;
v1[0] = pl[j].x - pl[i_cur].x;
v1[1] = pl[j].y - pl[i_cur].y;
v1[2] = pl[j].z - pl[i_cur].z;
v2[0] = v1[1] * vz - vy * v1[2];
v2[1] = v1[2] * vx - v1[0] * vz;
v2[2] = v1[0] * vy - vx * v1[1];
double lw = v2[0] * v2[0] + v2[1] * v2[1] + v2[2] * v2[2];
if (lw > leng_wid) {
leng_wid = lw;
}
}
if ((two_dis * two_dis / leng_wid) < p2l_ratio) {
curr_direct.setZero();
return 0;
}
uint disarrsize = disarr.size();
for (uint j = 0; j < disarrsize - 1; j++) {
for (uint k = j + 1; k < disarrsize; k++) {
if (disarr[j] < disarr[k]) {
leng_wid = disarr[j];
disarr[j] = disarr[k];
disarr[k] = leng_wid;
}
}
}
if (disarr[disarr.size() - 2] < 1e-16) {
curr_direct.setZero();
return 0;
}
if (lidar_type == AVIA) {
double dismax_mid = disarr[0] / disarr[disarrsize / 2];
double dismid_min = disarr[disarrsize / 2] / disarr[disarrsize - 2];
if (dismax_mid >= limit_maxmid || dismid_min >= limit_midmin) {
curr_direct.setZero();
return 0;
}
} else {
double dismax_min = disarr[0] / disarr[disarrsize - 2];
if (dismax_min >= limit_maxmin) {
curr_direct.setZero();
return 0;
}
}
curr_direct << vx, vy, vz;
curr_direct.normalize();
return 1;
}
bool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir) {
if (nor_dir == 0) {
if (types[i - 1].range < blind || types[i - 2].range < blind) {
return false;
}
} else if (nor_dir == 1) {
if (types[i + 1].range < blind || types[i + 2].range < blind) {
return false;
}
}
double d1 = types[i + nor_dir - 1].dista;
double d2 = types[i + 3 * nor_dir - 2].dista;
double d;
if (d1 < d2) {
d = d1;
d1 = d2;
d2 = d;
}
d1 = sqrt(d1);
d2 = sqrt(d2);
if (d1 > edgea * d2 || (d1 - d2) > edgeb) {
return false;
}
return true;
}
point_lio_ros2/src/preprocess.h
+193
View File
@@ -0,0 +1,193 @@
#include <rclcpp/rclcpp.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
// #include <livox_ros_driver2/msg/custom_msg.hpp>
using namespace std;
#define IS_VALID(a) ((abs(a)>1e8) ? true : false)
typedef pcl::PointXYZINormal PointType;
typedef pcl::PointCloud<PointType> PointCloudXYZI;
enum LID_TYPE {
AVIA = 1, VELO16, OUST64, HESAIxt32, UNILIDAR
}; //{1, 2, 3, 4}
enum TIME_UNIT {
SEC = 0, MS = 1, US = 2, NS = 3
};
enum Feature {
Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint
};
enum Surround {
Prev, Next
};
enum E_jump {
Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind
};
const bool time_list_cut_frame(PointType &x, PointType &y);
struct orgtype {
double range;
double dista;
double angle[2];
double intersect;
E_jump edj[2];
Feature ftype;
orgtype() {
range = 0;
edj[Prev] = Nr_nor;
edj[Next] = Nr_nor;
ftype = Nor;
intersect = 2;
}
};
namespace velodyne_ros {
struct EIGEN_ALIGN16 Point {
PCL_ADD_POINT4D;
float intensity;
float time;
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace velodyne_ros
POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
(float, time, time)
(std::uint16_t, ring, ring)
)
/**
* @brief Unilidar Point Type
*/
namespace unilidar_ros {
struct EIGEN_ALIGN16 Point {
PCL_ADD_POINT4D
PCL_ADD_INTENSITY
std::uint16_t ring;
float time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace unilidar_ros
POINT_CLOUD_REGISTER_POINT_STRUCT(unilidar_ros::Point,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
(std::uint16_t, ring, ring)
(float, time, time)
)
namespace hesai_ros {
struct EIGEN_ALIGN16 Point {
PCL_ADD_POINT4D;
float intensity;
double timestamp;
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace velodyne_ros
POINT_CLOUD_REGISTER_POINT_STRUCT(hesai_ros::Point,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
(double, timestamp, timestamp)
(std::uint16_t, ring, ring)
)
namespace ouster_ros {
struct EIGEN_ALIGN16 Point {
PCL_ADD_POINT4D;
float intensity;
uint32_t t;
uint16_t reflectivity;
uint8_t ring;
uint16_t ambient;
uint32_t range;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace ouster_ros
// clang-format off
POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
// use std::uint32_t to avoid conflicting with pcl::uint32_t
(std::uint32_t, t, t)
(std::uint16_t, reflectivity, reflectivity)
(std::uint8_t, ring, ring)
(std::uint16_t, ambient, ambient)
(std::uint32_t, range, range)
)
class Preprocess {
public:
// EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Preprocess();
~Preprocess();
// void process(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out);
void process(const sensor_msgs::msg::PointCloud2::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out);
void set(bool feat_en, int lid_type, double bld, int pfilt_num);
// sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud;
PointCloudXYZI pl_full, pl_corn, pl_surf;
PointCloudXYZI pl_buff[128]; //maximum 128 line lidar
vector<orgtype> typess[128]; //maximum 128 line lidar
float time_unit_scale;
int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit;
double blind;
bool given_offset_time;
//ros::Publisher pub_full, pub_surf, pub_corn;
private:
// void avia_handler(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg);
void oust64_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg);
void velodyne_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg);
void unilidar_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg);
void hesai_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg);
void give_feature(PointCloudXYZI &pl, vector<orgtype> &types);
void pub_func(PointCloudXYZI &pl, const rclcpp::Time &ct);
int
plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);
bool small_plane(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex,
Eigen::Vector3d &curr_direct);
bool edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir);
int group_size;
double disA, disB, inf_bound;
double limit_maxmid, limit_midmin, limit_maxmin;
double p2l_ratio;
double jump_up_limit, jump_down_limit;
double cos160;
double edgea, edgeb;
double smallp_intersect, smallp_ratio;
double vx, vy, vz;
};