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

add the preprocessing downsample

Browse Source
This commit is contained in:
buaaxw@gmail.com
2021-01-19 15:16:23 +08:00
parent d57fe8aeff
commit 320765dd6c
4 changed files with 25 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/laserMapping.cpp
View File

@@ -1040,16 +1040,19 @@ int main(int argc, char** argv)
// state += solution;
auto vec = state_propagat - state;
solution = K * (meas_vec - Hsub * vec.block<6,1>(0,0));
state = state_propagat + solution;
// solution = K * (meas_vec - Hsub * vec.block<6,1>(0,0));
// state = state_propagat + solution;
solution = K * meas_vec + vec - K * Hsub * vec.block<6,1>(0,0);
state += solution;
rot_add = solution.block<3,1>(0,0);
t_add = solution.block<3,1>(3,0);
flg_EKF_converged = false;
if (((rot_add.norm() * 57.3 - deltaR) < 0.01) \
&& ((t_add.norm() * 100 - deltaT) < 0.015))
if ((rot_add.norm() * 57.3 < 0.01) \
&& (t_add.norm() * 100 < 0.015))
{
flg_EKF_converged = true;
}