12345qiupeng
/
Fast-Lio
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add the preprocessing downsample

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

2
launch/mapping_avia.launch
View File

@ -20,7 +20,7 @@
<param name="edgeb" type="double" value="0.1"/> <param name="edgeb" type="double" value="0.1"/>
<param name="smallp_intersect" type="double" value="172.5"/> <param name="smallp_intersect" type="double" value="172.5"/>
<param name="smallp_ratio" type="double" value="1.2"/> <param name="smallp_ratio" type="double" value="1.2"/>
<param name="point_filter_num" type="int" value="1"/> <param name="point_filter_num" type="int" value="3"/>
<node pkg="fast_lio" type="loam_feat_extract" name="feature_extract" output="screen"/> <node pkg="fast_lio" type="loam_feat_extract" name="feature_extract" output="screen"/>
<node pkg="fast_lio" type="loam_laserMapping" name="laserMapping" output="screen" required="true"> <node pkg="fast_lio" type="loam_laserMapping" name="laserMapping" output="screen" required="true">

2
launch/mapping_avia_outdoor.launch
View File

@ -20,7 +20,7 @@
<param name="edgeb" type="double" value="0.1"/> <param name="edgeb" type="double" value="0.1"/>
<param name="smallp_intersect" type="double" value="172.5"/> <param name="smallp_intersect" type="double" value="172.5"/>
<param name="smallp_ratio" type="double" value="1.2"/> <param name="smallp_ratio" type="double" value="1.2"/>
<param name="point_filter_num" type="int" value="4"/> <param name="point_filter_num" type="int" value="3"/>
<node pkg="fast_lio" type="loam_feat_extract" name="feature_extract" output="screen"/> <node pkg="fast_lio" type="loam_feat_extract" name="feature_extract" output="screen"/>
<node pkg="fast_lio" type="loam_laserMapping" name="laserMapping" output="screen"> <node pkg="fast_lio" type="loam_laserMapping" name="laserMapping" output="screen">

32
src/feature_extract.cpp
View File

@ -792,23 +792,23 @@ void give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types, pcl::P
if(j == uint(last_surface+point_filter_num-1)) if(j == uint(last_surface+point_filter_num-1))
{ {
for(uint k=last_surface; k<=j; k++) // for(uint k=last_surface; k<=j; k++)
{ // {
PointType ap; // PointType ap;
ap.x = pl[k].x; // ap.x = pl[k].x;
ap.y = pl[k].y; // ap.y = pl[k].y;
ap.z = pl[k].z; // ap.z = pl[k].z;
ap.curvature = pl[k].curvature; // ap.curvature = pl[k].curvature;
ap.intensity = pl[k].intensity; // ap.intensity = pl[k].intensity;
pl_surf.push_back(ap); // pl_surf.push_back(ap);
} // }
// PointType ap; PointType ap;
// ap.x = pl[last_surface].x; ap.x = pl[j].x;
// ap.y = pl[last_surface].y; ap.y = pl[j].y;
// ap.z = pl[last_surface].z; ap.z = pl[j].z;
// ap.curvature += pl[last_surface].curvature; ap.curvature += pl[j].curvature;
// pl_surf.push_back(ap); pl_surf.push_back(ap);
last_surface = -1; last_surface = -1;
} }

11
src/laserMapping.cpp
View File

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