mirrors/FAST_LIO
1
0
Fork 0
mirror of https://github.com/hku-mars/FAST_LIO.git synced 2023-04-06 16:28:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

update the pcd saving to one map

Browse Source
This commit is contained in:
xw
2021-07-06 05:04:31 -04:00
parent bd8863926c
commit ae6a01383a
4 changed files with 25 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
src/laserMapping.cpp
View File

@@ -420,7 +420,7 @@ void map_incremental()
}
PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1));
int ind = 0;
PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI());
void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
{
PointCloudXYZI::Ptr laserCloudFullRes(dense_map_en ? feats_undistort : feats_down_body);
@@ -434,16 +434,16 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
&laserCloudWorld->points[i]);
}
*pcl_wait_pub += *laserCloudWorld;
// *pcl_wait_pub += *laserCloudWorld;
if(1)
{
sensor_msgs::PointCloud2 laserCloudmsg;
pcl::toROSMsg(*pcl_wait_pub, laserCloudmsg);
pcl::toROSMsg(*laserCloudWorld, laserCloudmsg);
laserCloudmsg.header.stamp = ros::Time::now();
laserCloudmsg.header.frame_id = "camera_init";
pubLaserCloudFullRes.publish(laserCloudmsg);
publish_count -= PUBFRAME_PERIOD;
pcl_wait_pub->clear();
// pcl_wait_pub->clear();
}
/**************** save map ****************/
@@ -451,13 +451,8 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
/* 2. pcd save will largely influence the real-time performences **/
if (laserCloudWorld->size() > 0 && pcd_save_en)
{
string file_name = string("scans_"+to_string(ind)+".pcd");
string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name);
pcl::PCDWriter pcd_writer;
cout << "current scan saved to /PCD/" << file_name<<endl;
pcd_writer.writeBinary(all_points_dir, *laserCloudWorld);
*pcl_wait_save += *laserCloudWorld;
}
ind ++;
}
void publish_effect_world(const ros::Publisher & pubLaserCloudEffect)
@@ -898,6 +893,18 @@ int main(int argc, char** argv)
rate.sleep();
}
/**************** save map ****************/
/* 1. make sure you have enough memories
/* 2. pcd save will largely influence the real-time performences **/
if (pcl_wait_save->size() > 0 && pcd_save_en)
{
string file_name = string("scans.pcd");
string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name);
pcl::PCDWriter pcd_writer;
cout << "current scan saved to /PCD/" << file_name<<endl;
pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
}
fout_out.close();
fout_pre.close();

3
src/preprocess.cpp
View File

@@ -724,6 +724,7 @@ void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &t
ap.x = pl[j].x;
ap.y = pl[j].y;
ap.z = pl[j].z;
ap.intensity = pl[j].intensity;
ap.curvature = pl[j].curvature;
pl_surf.push_back(ap);
@@ -744,11 +745,13 @@ void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &t
ap.x += pl[k].x;
ap.y += pl[k].y;
ap.z += pl[k].z;
ap.intensity += pl[k].intensity;
ap.curvature += pl[k].curvature;
}
ap.x /= (j-last_surface);
ap.y /= (j-last_surface);
ap.z /= (j-last_surface);
ap.intensity /= (j-last_surface);
ap.curvature /= (j-last_surface);
pl_surf.push_back(ap);
}