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="smallp_intersect" type="double" value="172.5"/>
<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_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="smallp_intersect" type="double" value="172.5"/>
<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_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))
{
for(uint k=last_surface; k<=j; k++)
{
PointType ap;
ap.x = pl[k].x;
ap.y = pl[k].y;
ap.z = pl[k].z;
ap.curvature = pl[k].curvature;
ap.intensity = pl[k].intensity;
pl_surf.push_back(ap);
}
// for(uint k=last_surface; k<=j; k++)
// {
// PointType ap;
// ap.x = pl[k].x;
// ap.y = pl[k].y;
// ap.z = pl[k].z;
// ap.curvature = pl[k].curvature;
// ap.intensity = pl[k].intensity;
// pl_surf.push_back(ap);
// }
// PointType ap;
// ap.x = pl[last_surface].x;
// ap.y = pl[last_surface].y;
// ap.z = pl[last_surface].z;
// ap.curvature += pl[last_surface].curvature;
// pl_surf.push_back(ap);
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;
}

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;
}