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

381
src/IMU_Processing.hpp
View File

@@ -21,12 +21,12 @@
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <fast_lio/States.h>
#include <geometry_msgs/Vector3.h>
#include "use-ikfom.hpp"
/// *************Preconfiguration
#define MAX_INI_COUNT (200)
#define MAX_INI_COUNT (20)
const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);};
@@ -38,116 +38,140 @@ class ImuProcess
ImuProcess();
~ImuProcess();
void Process(const MeasureGroup &meas, StatesGroup &state, PointCloudXYZI::Ptr pcl_un_);
void Reset();
void IMU_Initial(const MeasureGroup &meas, StatesGroup &state, int &N);
// Eigen::Matrix3d Exp(const Eigen::Vector3d &ang_vel, const double &dt);
void IntegrateGyr(const std::vector<sensor_msgs::Imu::ConstPtr> &v_imu);
void UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_in_out);
ros::NodeHandle nh;
void Integrate(const sensor_msgs::ImuConstPtr &imu);
void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu);
void set_extrinsic(const V3D &transl, const M3D &rot);
void set_extrinsic(const V3D &transl);
void set_extrinsic(const MD(4,4) &T);
void set_gyr_cov(const V3D &scaler);
void set_acc_cov(const V3D &scaler);
void set_gyr_bias_cov(const V3D &b_g);
void set_acc_bias_cov(const V3D &b_a);
Eigen::Matrix<double, 12, 12> Q;
void Process(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI::Ptr pcl_un_);
double scale_gravity;
Eigen::Vector3d angvel_last;
Eigen::Vector3d acc_s_last;
Eigen::Matrix<double,DIM_OF_PROC_N,1> cov_proc_noise;
Eigen::Vector3d cov_acc;
Eigen::Vector3d cov_gyr;
std::ofstream fout;
ofstream fout_imu;
V3D cov_acc;
V3D cov_gyr;
V3D cov_acc_scale;
V3D cov_gyr_scale;
V3D cov_bias_gyr;
V3D cov_bias_acc;
double first_lidar_time;
private:
/*** Whether is the first frame, init for first frame ***/
bool b_first_frame_ = true;
bool imu_need_init_ = true;
void IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N);
void UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI &pcl_in_out);
int init_iter_num = 1;
Eigen::Vector3d mean_acc;
Eigen::Vector3d mean_gyr;
/*** Undistorted pointcloud ***/
PointCloudXYZI::Ptr cur_pcl_un_;
//// For timestamp usage
sensor_msgs::ImuConstPtr last_imu_;
/*** For gyroscope integration ***/
deque<sensor_msgs::ImuConstPtr> v_imu_;
vector<Pose6D> IMUpose;
vector<M3D> v_rot_pcl_;
M3D Lidar_R_wrt_IMU;
V3D Lidar_T_wrt_IMU;
V3D mean_acc;
V3D mean_gyr;
V3D angvel_last;
V3D acc_s_last;
double start_timestamp_;
/// Making sure the equal size: v_imu_ and v_rot_
std::deque<sensor_msgs::ImuConstPtr> v_imu_;
std::vector<Eigen::Matrix3d> v_rot_pcl_;
std::vector<Pose6D> IMUpose;
double last_lidar_end_time_;
int init_iter_num = 1;
bool b_first_frame_ = true;
bool imu_need_init_ = true;
};
ImuProcess::ImuProcess()
: b_first_frame_(true), imu_need_init_(true), last_imu_(nullptr), start_timestamp_(-1)
: b_first_frame_(true), imu_need_init_(true), start_timestamp_(-1)
{
Eigen::Quaterniond q(0, 1, 0, 0);
Eigen::Vector3d t(0, 0, 0);
init_iter_num = 1;
scale_gravity = 1.0;
cov_acc = Eigen::Vector3d(0.1, 0.1, 0.1);
cov_gyr = Eigen::Vector3d(0.1, 0.1, 0.1);
mean_acc = Eigen::Vector3d(0, 0, -1.0);
mean_gyr = Eigen::Vector3d(0, 0, 0);
angvel_last = Zero3d;
cov_proc_noise = Eigen::Matrix<double,DIM_OF_PROC_N,1>::Zero();
// Lidar_offset_to_IMU = Eigen::Vector3d(0.0, 0.0, -0.0);
// fout.open(DEBUG_FILE_DIR("imu.txt"),std::ios::out);
Q = process_noise_cov();
cov_acc = V3D(0.1, 0.1, 0.1);
cov_gyr = V3D(0.1, 0.1, 0.1);
cov_bias_gyr = V3D(0.0001, 0.0001, 0.0001);
cov_bias_acc = V3D(0.0001, 0.0001, 0.0001);
mean_acc = V3D(0, 0, -1.0);
mean_gyr = V3D(0, 0, 0);
angvel_last = Zero3d;
Lidar_T_wrt_IMU = Zero3d;
Lidar_R_wrt_IMU = Eye3d;
last_imu_.reset(new sensor_msgs::Imu());
}
ImuProcess::~ImuProcess() {fout.close();}
ImuProcess::~ImuProcess() {}
void ImuProcess::Reset()
{
ROS_WARN("Reset ImuProcess");
scale_gravity = 1.0;
angvel_last = Zero3d;
cov_proc_noise = Eigen::Matrix<double,DIM_OF_PROC_N,1>::Zero();
cov_acc = Eigen::Vector3d(0.1, 0.1, 0.1);
cov_gyr = Eigen::Vector3d(0.1, 0.1, 0.1);
mean_acc = Eigen::Vector3d(0, 0, -1.0);
mean_gyr = Eigen::Vector3d(0, 0, 0);
imu_need_init_ = true;
b_first_frame_ = true;
init_iter_num = 1;
last_imu_ = nullptr;
//gyr_int_.Reset(-1, nullptr);
start_timestamp_ = -1;
mean_acc = V3D(0, 0, -1.0);
mean_gyr = V3D(0, 0, 0);
angvel_last = Zero3d;
imu_need_init_ = true;
start_timestamp_ = -1;
init_iter_num = 1;
v_imu_.clear();
IMUpose.clear();
last_imu_.reset(new sensor_msgs::Imu());
cur_pcl_un_.reset(new PointCloudXYZI());
fout.close();
}
void ImuProcess::IMU_Initial(const MeasureGroup &meas, StatesGroup &state_inout, int &N)
void ImuProcess::set_extrinsic(const MD(4,4) &T)
{
Lidar_T_wrt_IMU = T.block<3,1>(0,3);
Lidar_R_wrt_IMU = T.block<3,3>(0,0);
}
void ImuProcess::set_extrinsic(const V3D &transl)
{
Lidar_T_wrt_IMU = transl;
Lidar_R_wrt_IMU.setIdentity();
}
void ImuProcess::set_extrinsic(const V3D &transl, const M3D &rot)
{
Lidar_T_wrt_IMU = transl;
Lidar_R_wrt_IMU = rot;
}
void ImuProcess::set_gyr_cov(const V3D &scaler)
{
cov_gyr_scale = scaler;
}
void ImuProcess::set_acc_cov(const V3D &scaler)
{
cov_acc_scale = scaler;
}
void ImuProcess::set_gyr_bias_cov(const V3D &b_g)
{
cov_bias_gyr = b_g;
}
void ImuProcess::set_acc_bias_cov(const V3D &b_a)
{
cov_bias_acc = b_a;
}
void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N)
{
/** 1. initializing the gravity, gyro bias, acc and gyro covariance
** 2. normalize the acceleration measurenments to unit gravity **/
ROS_INFO("IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100);
Eigen::Vector3d cur_acc, cur_gyr;
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;
first_lidar_time = meas.lidar_beg_time;
// cout<<"init acc norm: "<<mean_acc.norm()<<endl;
}
for (const auto &imu : meas.imu)
@@ -157,22 +181,37 @@ void ImuProcess::IMU_Initial(const MeasureGroup &meas, StatesGroup &state_inout,
cur_acc << imu_acc.x, imu_acc.y, imu_acc.z;
cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
scale_gravity += (cur_acc.norm() - scale_gravity) / N;
mean_acc += (cur_acc - mean_acc) / N;
mean_gyr += (cur_gyr - mean_gyr) / N;
cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N);
cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) * (N - 1.0) / (N * N);
// cout<<"acc norm: "<<cur_acc.norm()<<" "<<mean_acc.norm()<<endl;
N ++;
}
state_ikfom init_state = kf_state.get_x();
init_state.grav = S2(- mean_acc / mean_acc.norm() * G_m_s2);
//state_inout.rot = Eye3d; // Exp(mean_acc.cross(V3D(0, 0, -1 / scale_gravity)));
init_state.bg = mean_gyr;
init_state.offset_T_L_I = Lidar_T_wrt_IMU;
init_state.offset_R_L_I = Lidar_R_wrt_IMU;
kf_state.change_x(init_state);
state_inout.gravity = - mean_acc /scale_gravity * G_m_s2;
state_inout.rot_end = Eye3d; // Exp(mean_acc.cross(Eigen::Vector3d(0, 0, -1 / scale_gravity)));
state_inout.bias_g = mean_gyr;
esekfom::esekf<state_ikfom, 12, input_ikfom>::cov init_P = kf_state.get_P();
init_P.setIdentity();
init_P(6,6) = init_P(7,7) = init_P(8,8) = 0.00001;
init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001;
init_P(15,15) = init_P(16,16) = init_P(17,17) = 0.0001;
init_P(18,18) = init_P(19,19) = init_P(20,20) = 0.001;
init_P(21,21) = init_P(22,22) = 0.00001;
kf_state.change_P(init_P);
last_imu_ = meas.imu.back();
}
void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out)
void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI &pcl_out)
{
/*** add the imu of the last frame-tail to the of current frame-head ***/
auto v_imu = meas.imu;
@@ -183,27 +222,30 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout
/*** sort point clouds by offset time ***/
pcl_out = *(meas.lidar);
std::sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);
sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);
const double &pcl_end_time = pcl_beg_time + pcl_out.points.back().curvature / double(1000);
std::cout<<"[ IMU Process ]: Process lidar from "<<pcl_beg_time<<" to "<<pcl_end_time<<", " \
<<meas.imu.size()<<" imu msgs from "<<imu_beg_time<<" to "<<imu_end_time<<std::endl;
// cout<<"[ IMU Process ]: Process lidar from "<<pcl_beg_time<<" to "<<pcl_end_time<<", " \
// <<meas.imu.size()<<" imu msgs from "<<imu_beg_time<<" to "<<imu_end_time<<endl;
/*** Initialize IMU pose ***/
state_ikfom imu_state = kf_state.get_x();
IMUpose.clear();
// IMUpose.push_back(set_pose6d(0.0, Zero3d, Zero3d, state.vel_end, state.pos_end, state.rot_end));
IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, state_inout.vel_end, state_inout.pos_end, state_inout.rot_end));
IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));
/*** forward propagation at each imu point ***/
Eigen::Vector3d acc_imu, angvel_avr, acc_avr, vel_imu(state_inout.vel_end), pos_imu(state_inout.pos_end);
Eigen::Matrix3d R_imu(state_inout.rot_end);
Eigen::MatrixXd F_x(Eigen::Matrix<double, DIM_OF_STATES, DIM_OF_STATES>::Identity());
Eigen::MatrixXd cov_w(Eigen::Matrix<double, DIM_OF_STATES, DIM_OF_STATES>::Zero());
V3D angvel_avr, acc_avr, acc_imu, vel_imu, pos_imu;
M3D R_imu;
double dt = 0;
for (auto it_imu = v_imu.begin(); it_imu != (v_imu.end() - 1); it_imu++)
input_ikfom in;
for (auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++)
{
auto &&head = *(it_imu);
auto &&tail = *(it_imu + 1);
if (tail->header.stamp.toSec() < last_lidar_end_time_) continue;
angvel_avr<<0.5 * (head->angular_velocity.x + tail->angular_velocity.x),
0.5 * (head->angular_velocity.y + tail->angular_velocity.y),
0.5 * (head->angular_velocity.z + tail->angular_velocity.z);
@@ -211,71 +253,48 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout
0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y),
0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z);
angvel_avr -= state_inout.bias_g;
acc_avr = acc_avr * G_m_s2 / scale_gravity - state_inout.bias_a;
// fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl;
#ifdef DEBUG_PRINT
// fout<<head->header.stamp.toSec()<<" "<<angvel_avr.transpose()<<" "<<acc_avr.transpose()<<std::endl;
#endif
dt = tail->header.stamp.toSec() - head->header.stamp.toSec();
acc_avr = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba;
if(head->header.stamp.toSec() < last_lidar_end_time_)
{
dt = tail->header.stamp.toSec() - last_lidar_end_time_;
// dt = tail->header.stamp.toSec() - pcl_beg_time;
}
else
{
dt = tail->header.stamp.toSec() - head->header.stamp.toSec();
}
/* covariance propagation */
Eigen::Matrix3d acc_avr_skew;
Eigen::Matrix3d Exp_f = Exp(angvel_avr, dt);
acc_avr_skew<<SKEW_SYM_MATRX(angvel_avr);
F_x.block<3,3>(0,0) = Exp(angvel_avr, - dt);
F_x.block<3,3>(0,9) = - Eye3d * dt;
// F_x.block<3,3>(3,0) = R_imu * off_vel_skew * dt;
F_x.block<3,3>(3,6) = Eye3d * dt;
F_x.block<3,3>(6,0) = - R_imu * acc_avr_skew * dt;
F_x.block<3,3>(6,12) = - R_imu * dt;
F_x.block<3,3>(6,15) = Eye3d * dt;
Eigen::Matrix3d cov_acc_diag(Eye3d), cov_gyr_diag(Eye3d);
cov_acc_diag.diagonal() = cov_acc;
cov_gyr_diag.diagonal() = cov_gyr;
cov_w.block<3,3>(0,0).diagonal() = cov_gyr * dt * dt * 10000;
cov_w.block<3,3>(3,3) = R_imu * cov_gyr_diag * R_imu.transpose() * dt * dt * 10000;
cov_w.block<3,3>(6,6) = R_imu * cov_acc_diag * R_imu.transpose() * dt * dt * 10000;
cov_w.block<3,3>(9,9).diagonal() = Eigen::Vector3d(0.0001, 0.0001, 0.0001) * dt * dt; // bias gyro covariance
cov_w.block<3,3>(12,12).diagonal() = Eigen::Vector3d(0.0001, 0.0001, 0.0001) * dt * dt; // bias acc covariance
state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w;
/* propogation of IMU attitude */
R_imu = R_imu * Exp_f;
/* Specific acceleration (global frame) of IMU */
acc_imu = R_imu * acc_avr + state_inout.gravity;
/* propogation of IMU */
pos_imu = pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt;
/* velocity of IMU */
vel_imu = vel_imu + acc_imu * dt;
in.acc = acc_avr;
in.gyro = angvel_avr;
Q.block<3, 3>(0, 0).diagonal() = cov_gyr;
Q.block<3, 3>(3, 3).diagonal() = cov_acc;
Q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr;
Q.block<3, 3>(9, 9).diagonal() = cov_bias_acc;
kf_state.predict(dt, Q, in);
/* save the poses at each IMU measurements */
angvel_last = angvel_avr;
acc_s_last = acc_imu;
imu_state = kf_state.get_x();
angvel_last = angvel_avr - imu_state.bg;
acc_s_last = imu_state.rot * (acc_avr - imu_state.ba);
for(int i=0; i<3; i++)
{
acc_s_last[i] += imu_state.grav[i];
}
double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time;
// std::cout<<"acc "<<acc_imu.transpose()<<"vel "<<acc_imu.transpose()<<"vel "<<pos_imu.transpose()<<std::endl;
IMUpose.push_back(set_pose6d(offs_t, acc_imu, angvel_avr, vel_imu, pos_imu, R_imu));
IMUpose.push_back(set_pose6d(offs_t, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));
}
/*** calculated the pos and attitude prediction at the frame-end ***/
dt = pcl_end_time - imu_end_time;
state_inout.vel_end = vel_imu + acc_imu * dt;
state_inout.rot_end = R_imu * Exp(angvel_avr, dt);
state_inout.pos_end = pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt;
auto pos_liD_e = state_inout.pos_end + state_inout.rot_end * Lidar_offset_to_IMU;
// auto R_liD_e = state_inout.rot_end * Lidar_R_to_IMU;
#ifdef DEBUG_PRINT
std::cout<<"[ IMU Process ]: vel "<<state_inout.vel_end.transpose()<<" pos "<<state_inout.pos_end.transpose()<<" ba"<<state_inout.bias_a.transpose()<<" bg "<<state_inout.bias_g.transpose()<<std::endl;
std::cout<<"propagated cov: "<<state_inout.cov.diagonal().transpose()<<std::endl;
#endif
double note = pcl_end_time > imu_end_time ? 1.0 : -1.0;
dt = note * (pcl_end_time - imu_end_time);
kf_state.predict(dt, Q, in);
imu_state = kf_state.get_x();
last_imu_ = meas.imu.back();
last_lidar_end_time_ = pcl_end_time;
/*** undistort each lidar point (backward propagation) ***/
auto it_pcl = pcl_out.points.end() - 1;
@@ -284,11 +303,11 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout
auto head = it_kp - 1;
auto tail = it_kp;
R_imu<<MAT_FROM_ARRAY(head->rot);
acc_imu<<VEC_FROM_ARRAY(head->acc);
// std::cout<<"head imu acc: "<<acc_imu.transpose()<<std::endl;
// cout<<"head imu acc: "<<acc_imu.transpose()<<endl;
vel_imu<<VEC_FROM_ARRAY(head->vel);
pos_imu<<VEC_FROM_ARRAY(head->pos);
angvel_avr<<VEC_FROM_ARRAY(head->gyr);
acc_imu<<VEC_FROM_ARRAY(tail->acc);
angvel_avr<<VEC_FROM_ARRAY(tail->gyr);
for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --)
{
@@ -298,13 +317,13 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout
* Note: Compensation direction is INVERSE of Frame's moving direction
* So if we want to compensate a point at timestamp-i to the frame-e
* P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */
Eigen::Matrix3d R_i(R_imu * Exp(angvel_avr, dt));
Eigen::Vector3d T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt + R_i * Lidar_offset_to_IMU - pos_liD_e);
Eigen::Vector3d P_i(it_pcl->x, it_pcl->y, it_pcl->z);
Eigen::Vector3d P_compensate = state_inout.rot_end.transpose() * (R_i * P_i + T_ei);
/// save Undistorted points and their rotation
M3D R_i(R_imu * Exp(angvel_avr, dt));
V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z);
V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos);
V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate!
// save Undistorted points and their rotation
it_pcl->x = P_compensate(0);
it_pcl->y = P_compensate(1);
it_pcl->z = P_compensate(2);
@@ -314,54 +333,48 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout
}
}
void ImuProcess::Process(const MeasureGroup &meas, StatesGroup &stat, PointCloudXYZI::Ptr cur_pcl_un_)
void ImuProcess::Process(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI::Ptr cur_pcl_un_)
{
double t1,t2,t3;
t1 = omp_get_wtime();
if(meas.imu.empty()) {std::cout<<"no imu data"<<std::endl;return;};
if(meas.imu.empty()) {return;};
ROS_ASSERT(meas.lidar != nullptr);
if (imu_need_init_)
{
/// The very first lidar frame
IMU_Initial(meas, stat, init_iter_num);
IMU_init(meas, kf_state, init_iter_num);
imu_need_init_ = true;
last_imu_ = meas.imu.back();
state_ikfom imu_state = kf_state.get_x();
if (init_iter_num > MAX_INI_COUNT)
{
cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2);
imu_need_init_ = false;
// std::cout<<"mean acc: "<<mean_acc<<" acc measures in word frame:"<<state.rot_end.transpose()*mean_acc<<std::endl;
ROS_INFO("IMU Initials: Gravity: %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\
stat.gravity[0], stat.gravity[1], stat.gravity[2], stat.bias_g[0], stat.bias_g[1], stat.bias_g[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]);
// ROS_INFO("IMU Initials: Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\
// imu_state.grav[0], imu_state.grav[1], imu_state.grav[2], mean_acc.norm(), cov_acc_scale[0], cov_acc_scale[1], cov_acc_scale[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]);
// cov_acc = cov_acc.cwiseProduct(cov_acc_scale);
// cov_gyr = cov_gyr.cwiseProduct(cov_gyr_scale);
cov_acc = cov_acc_scale;
cov_gyr = cov_gyr_scale;
// cout<<"mean acc: "<<mean_acc<<" acc measures in word frame:"<<state.rot_end.transpose()*mean_acc<<endl;
ROS_INFO("IMU Initials: Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\
imu_state.grav[0], imu_state.grav[1], imu_state.grav[2], mean_acc.norm(), cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]);
fout_imu.open(DEBUG_FILE_DIR("imu.txt"),ios::out);
}
return;
}
/// Undistort points： the first point is assummed as the base frame
/// Compensate lidar points with IMU rotation (with only rotation now)
UndistortPcl(meas, stat, *cur_pcl_un_);
UndistortPcl(meas, kf_state, *cur_pcl_un_);
t2 = omp_get_wtime();
// {
// static ros::Publisher pub_UndistortPcl =
// nh.advertise<sensor_msgs::PointCloud2>("/livox_undistort", 100);
// sensor_msgs::PointCloud2 pcl_out_msg;
// pcl::toROSMsg(*cur_pcl_un_, pcl_out_msg);
// pcl_out_msg.header.stamp = ros::Time().fromSec(meas.lidar_beg_time);
// pcl_out_msg.header.frame_id = "/livox";
// pub_UndistortPcl.publish(pcl_out_msg);
// }
/// Record last measurements
last_imu_ = meas.imu.back();
t3 = omp_get_wtime();
std::cout<<"[ IMU Process ]: Time: "<<t3 - t1<<std::endl;
}
// cout<<"[ IMU Process ]: Time: "<<t3 - t1<<endl;
}

1008
src/feature_extract.cpp
View File

File diff suppressed because it is too large Load Diff

1734
src/laserMapping.cpp
View File

File diff suppressed because it is too large Load Diff

920
src/preprocess.cpp Normal file
View File

@@ -0,0 +1,920 @@
#include "preprocess.h"
#define RETURN0 0x00
#define RETURN0AND1 0x10
Preprocess::Preprocess()
:feature_enabled(0), lidar_type(AVIA), blind(0.01), point_filter_num(1)
{
inf_bound = 10;
N_SCANS = 6;
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)
{
feature_enabled = feat_en;
lidar_type = lid_type;
blind = bld;
point_filter_num = pfilt_num;
}
void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
{
avia_handler(msg);
*pcl_out = pl_surf;
}
void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
{
switch (lidar_type)
{
case OUST64:
oust64_handler(msg);
break;
case VELO16:
velodyne_handler(msg);
break;
default:
printf("Error LiDAR Type");
break;
}
*pcl_out = pl_surf;
}
void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
{
pl_surf.clear();
pl_corn.clear();
pl_full.clear();
double t1 = omp_get_wtime();
int plsize = msg->point_num;
// cout<<"plsie: "<<plsize<<endl;
pl_corn.reserve(plsize);
pl_surf.reserve(plsize);
pl_full.resize(plsize);
for(int i=0; i<N_SCANS; i++)
{
pl_buff[i].clear();
pl_buff[i].reserve(plsize);
}
uint valid_num = 0;
if (feature_enabled)
{
for(uint i=1; i<plsize; i++)
{
if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10))
{
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
bool is_new = false;
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_buff[msg->points[i].line].push_back(pl_full[i]);
}
}
}
static int count = 0;
static double time = 0.0;
count ++;
double t0 = omp_get_wtime();
for(int j=0; j<N_SCANS; j++)
{
if(pl_buff[j].size() <= 5) continue;
pcl::PointCloud<PointType> &pl = pl_buff[j];
plsize = pl.size();
vector<orgtype> &types = typess[j];
types.clear();
types.resize(plsize);
plsize--;
for(uint i=0; i<plsize; i++)
{
types[i].range = pl[i].x * pl[i].x + pl[i].y * pl[i].y;
vx = pl[i].x - pl[i + 1].x;
vy = pl[i].y - pl[i + 1].y;
vz = pl[i].z - pl[i + 1].z;
types[i].dista = vx * vx + vy * vy + vz * vz;
}
types[plsize].range = pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y;
give_feature(pl, types);
// pl_surf += pl;
}
time += omp_get_wtime() - t0;
printf("Feature extraction time: %lf \n", time / count);
}
else
{
for(uint i=1; i<plsize; i++)
{
if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10))
{
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
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 > blind))
{
pl_surf.push_back(pl_full[i]);
}
}
}
}
}
}
void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &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);
if (feature_enabled)
{
for (int i = 0; i < N_SCANS; i++)
{
pl_buff[i].clear();
pl_buff[i].reserve(plsize);
}
for (uint i = 0; i < plsize; i++)
{
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) 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;
double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;
if (yaw_angle >= 180.0)
yaw_angle -= 360.0;
if (yaw_angle <= -180.0)
yaw_angle += 360.0;
added_pt.curvature = pl_orig.points[i].t / 1e6;
if(pl_orig.points[i].ring < N_SCANS)
{
pl_buff[pl_orig.points[i].ring].push_back(added_pt);
}
}
for (int j = 0; j < N_SCANS; j++)
{
PointCloudXYZI &pl = pl_buff[j];
int linesize = pl.size();
vector<orgtype> &types = typess[j];
types.clear();
types.resize(linesize);
linesize--;
for (uint i = 0; i < linesize; i++)
{
types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);
vx = pl[i].x - pl[i + 1].x;
vy = pl[i].y - pl[i + 1].y;
vz = pl[i].z - pl[i + 1].z;
types[i].dista = vx * vx + vy * vy + vz * vz;
}
types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y);
give_feature(pl, types);
}
}
else
{
double time_stamp = msg->header.stamp.toSec();
// 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) 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;
double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;
if (yaw_angle >= 180.0)
yaw_angle -= 360.0;
if (yaw_angle <= -180.0)
yaw_angle += 360.0;
added_pt.curvature = pl_orig.points[i].t / 1e6;
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);
}
#define MAX_LINE_NUM 64
void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &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();
pl_surf.reserve(plsize);
bool is_first[MAX_LINE_NUM];
double yaw_fp[MAX_LINE_NUM]={0}; // yaw of first scan point
double omega_l=3.61; // scan angular velocity
float yaw_last[MAX_LINE_NUM]={0.0}; // yaw of last scan point
float time_last[MAX_LINE_NUM]={0.0}; // last offset time
if (pl_orig.points[plsize - 1].t > 0)
{
given_offset_time = true;
}
else
{
given_offset_time = false;
memset(is_first, true, sizeof(is_first));
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;
}
}
}
if(feature_enabled)
{
for (int i = 0; i < N_SCANS; i++)
{
pl_buff[i].clear();
pl_buff[i].reserve(plsize);
}
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;
int layer = pl_orig.points[i].ring;
if (layer >= N_SCANS) continue;
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].t / 1000.0; // units: ms
if (!given_offset_time)
{
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;
}
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;
}
pl_buff[layer].points.push_back(added_pt);
}
for (int j = 0; j < N_SCANS; j++)
{
PointCloudXYZI &pl = pl_buff[j];
int linesize = pl.size();
if (linesize < 2) continue;
vector<orgtype> &types = typess[j];
types.clear();
types.resize(linesize);
linesize--;
for (uint i = 0; i < linesize; i++)
{
types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);
vx = pl[i].x - pl[i + 1].x;
vy = pl[i].y - pl[i + 1].y;
vz = pl[i].z - pl[i + 1].z;
types[i].dista = vx * vx + vy * vy + vz * vz;
}
types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y);
give_feature(pl, types);
}
}
else
{
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].t / 1000.0;
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;
// added_pt.curvature = pl_orig.points[i].t;
yaw_last[layer] = yaw_angle;
time_last[layer]=added_pt.curvature;
}
// if(i==(plsize-1)) printf("index: %d layer: %d, yaw: %lf, offset-time: %lf, condition: %d\n", i, layer, yaw_angle, added_pt.curvature, prints);
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)
{
pl_surf.points.push_back(added_pt);
// printf("time mode: %d time: %d \n", given_offset_time, pl_orig.points[i].t);
}
}
}
}
// pub_func(pl_surf, pub_full, msg->header.stamp);
// pub_func(pl_surf, pub_surf, msg->header.stamp);
// pub_func(pl_surf, pub_corn, msg->header.stamp);
}
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;
}
// else if(plane_type == 0)
// {
// if(last_state == 1)
// {
// uint i_nex_tem;
// uint j;
// for(j=last_i+1; j<=last_i_nex; j++)
// {
// uint i_nex_tem2 = i_nex_tem;
// Eigen::Vector3d curr_direct2;
// uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2);
// if(ttem != 1)
// {
// i_nex_tem = i_nex_tem2;
// break;
// }
// curr_direct = curr_direct2;
// }
// if(j == last_i+1)
// {
// last_state = 0;
// }
// else
// {
// for(uint k=last_i_nex; k<=i_nex_tem; k++)
// {
// if(k != i_nex_tem)
// {
// types[k].ftype = Real_Plane;
// }
// else
// {
// types[k].ftype = Poss_Plane;
// }
// }
// i = i_nex_tem-1;
// i_nex = i_nex_tem;
// i2 = j-1;
// last_state = 1;
// }
// }
// }
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.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.curvature += pl[k].curvature;
}
ap.x /= (j-last_surface);
ap.y /= (j-last_surface);
ap.z /= (j-last_surface);
ap.curvature /= (j-last_surface);
pl_surf.push_back(ap);
}
last_surface = -1;
}
}
}
void Preprocess::pub_func(PointCloudXYZI &pl, const ros::Time &ct)
{
pl.height = 1; pl.width = pl.size();
sensor_msgs::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;
}

122
src/preprocess.h Normal file
View File

@@ -0,0 +1,122 @@
#include <ros/ros.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <livox_ros_driver/CustomMsg.h>
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}; //{1, 2, 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};
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;
uint32_t t;
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)
(uint32_t, t, t)
(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_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
void set(bool feat_en, int lid_type, double bld, int pfilt_num);
// sensor_msgs::PointCloud2::ConstPtr pointcloud;
PointCloudXYZI pl_full, pl_corn, pl_surf;
PointCloudXYZI pl_buff[128]; //maximum 128 line lidar
vector<orgtype> typess[128]; //maximum 128 line lidar
int lidar_type, point_filter_num, N_SCANS;;
double blind;
bool feature_enabled, given_offset_time;
ros::Publisher pub_full, pub_surf, pub_corn;
private:
void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg);
void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
void give_feature(PointCloudXYZI &pl, vector<orgtype> &types);
void pub_func(PointCloudXYZI &pl, const ros::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;
};