admin/Point-LIO_ROS2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix a bug for unstationary start

Browse Source
This commit is contained in:
Joanna-HE
2024-05-06 03:28:07 -04:00
parent 9de9b4a7b9
commit c13454d862
5 changed files with 15 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
config/avia.yaml
View File

@@ -34,8 +34,8 @@ mapping:
match_s: 81
ivox_grid_resolution: 2.0
gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # [0.0, 0.0, -9.787561] # gvins #
gravity_init: [0.0, 0.0, -9.810] # preknown gravity for unstationary start
extrinsic_T: [ 0.04165, 0.02326, -0.0284 ]
gravity_init: [0.0, 0.0, -9.810] # preknown gravity for unstationary start in the initial IMU frame
extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] # avia # [0.011, 0.02329, -0.04412] # mid360
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1 ]

2
include/common_lib.h
View File

@@ -73,7 +73,7 @@ extern esekfom::esekf<state_output, 30, input_ikfom> kf_output;
#define PBSTR "||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||"
#define PI_M (3.14159265358)
#define G_m_s2 (9.81) // Gravaty const in GuangDong/China
// #define G_m_s2 (9.81) // Gravaty const in GuangDong/China
#define DIM_STATE (24) // Dimension of states (Let Dim(SO(3)) = 3)
#define DIM_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3)
#define CUBE_LEN (6.0)

1
src/Estimator.cpp
View File

@@ -21,6 +21,7 @@ V3D angvel_avr, acc_avr, acc_avr_norm;
int feats_down_size = 0;
V3D Lidar_T_wrt_IMU(Zero3d);
M3D Lidar_R_wrt_IMU(Eye3d);
double G_m_s2 = 9.81;
Eigen::Matrix<double, 24, 24> process_noise_cov_input()
{

2
src/Estimator.h
View File

@@ -29,7 +29,7 @@ extern int feats_down_size;
// extern std::vector<Eigen::Vector3d> normvec_holder;
extern V3D Lidar_T_wrt_IMU; //(Zero3d);
extern M3D Lidar_R_wrt_IMU; //(Eye3d);
extern double G_m_s2;
extern input_ikfom input_in;
Eigen::Matrix<double, 24, 24> process_noise_cov_input();

19
src/laserMapping.cpp
View File

@@ -461,13 +461,14 @@ int main(int argc, char** argv)
}
else
{
kf_input.x_.gravity << VEC_FROM_ARRAY(gravity_init);
kf_output.x_.gravity << VEC_FROM_ARRAY(gravity_init);
kf_output.x_.acc << VEC_FROM_ARRAY(gravity_init);
kf_input.x_.gravity << VEC_FROM_ARRAY(gravity); // _init);
kf_output.x_.gravity << VEC_FROM_ARRAY(gravity); //_init);
kf_output.x_.acc << VEC_FROM_ARRAY(gravity); //_init);
kf_output.x_.acc *= -1;
p_imu->imu_need_init_ = false;
// p_imu->after_imu_init_ = true;
}
}
G_m_s2 = std::sqrt(gravity[0] * gravity[0] + gravity[1] * gravity[1] + gravity[2] * gravity[2]);
}
double t0,t1,t2,t3,t4,t5,match_start, solve_start;
@@ -495,21 +496,21 @@ int main(int argc, char** argv)
time_seq = time_compressing<int>(feats_down_body);
feats_down_size = feats_down_body->points.size();
}
if (!p_imu->after_imu_init_)
if (!p_imu->after_imu_init_) // !p_imu->UseLIInit &&
{
if (!p_imu->imu_need_init_)
{
V3D tmp_gravity;
if (imu_en)
{tmp_gravity = - p_imu->mean_acc / acc_norm * G_m_s2;}
{tmp_gravity = - p_imu->mean_acc / p_imu->mean_acc.norm() * G_m_s2;}
else
{tmp_gravity << VEC_FROM_ARRAY(gravity_init);
p_imu->after_imu_init_ = true;
}
// V3D tmp_gravity << VEC_FROM_ARRAY(gravity_init);
M3D rot_init;
p_imu->Set_init(tmp_gravity, rot_init);
p_imu->Set_init(tmp_gravity, rot_init);
kf_input.x_.rot = rot_init;
kf_output.x_.rot = rot_init;
// kf_input.x_.rot; //.normalize();
@@ -786,7 +787,7 @@ int main(int argc, char** argv)
angvel_avr<<imu_next.angular_velocity.x, imu_next.angular_velocity.y, imu_next.angular_velocity.z;
acc_avr <<imu_next.linear_acceleration.x, imu_next.linear_acceleration.y, imu_next.linear_acceleration.z;
acc_avr_norm = acc_avr * G_m_s2 / acc_norm;
// acc_avr_norm = acc_avr * G_m_s2 / acc_norm;
kf_output.update_iterated_dyn_share_IMU();
imu_deque.pop_front();
if (imu_deque.empty()) break;