i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 10:00:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

floam: republish input scan with features

Browse Source
This commit is contained in:
matlabbe
2022-10-16 21:31:43 -07:00
parent 5b0047efad
commit 6e8f43916c
2 changed files with 6 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
corelib/src/odometry/OdometryFLOAM.cpp
View File

@@ -70,7 +70,6 @@ OdometryFLOAM::OdometryFLOAM(const ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kIcpRangeMin(), min_dis); Parameters::parse(parameters, Parameters::kIcpRangeMin(), min_dis);
Parameters::parse(parameters, Parameters::kOdomLOAMResolution(), map_resolution); Parameters::parse(parameters, Parameters::kOdomLOAMResolution(), map_resolution);
UASSERT(scan_period>0.0f);
Parameters::parse(parameters, Parameters::kOdomLOAMLinVar(), linVar_); Parameters::parse(parameters, Parameters::kOdomLOAMLinVar(), linVar_);
UASSERT(linVar_>0.0f); UASSERT(linVar_>0.0f);
Parameters::parse(parameters, Parameters::kOdomLOAMAngVar(), angVar_); Parameters::parse(parameters, Parameters::kOdomLOAMAngVar(), angVar_);
@@ -140,6 +139,12 @@ Transform OdometryFLOAM::computeTransform(
laserProcessing_->featureExtraction(laserCloudInPtr,pointcloud_edge,pointcloud_surf); laserProcessing_->featureExtraction(laserCloudInPtr,pointcloud_edge,pointcloud_surf);
UDEBUG("Feature extraction: %fs", timer.ticks()); UDEBUG("Feature extraction: %fs", timer.ticks());
// Put back the laser scan filtered
pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud_filtered(new pcl::PointCloud<pcl::PointXYZI>());
*pointcloud_filtered+=*pointcloud_edge;
*pointcloud_filtered+=*pointcloud_surf;
data.setLaserScan(util3d::laserScanFromPointCloud(*pointcloud_filtered));
if(this->framesProcessed() == 0){ if(this->framesProcessed() == 0){
odomEstimation_->initMapWithPoints(pointcloud_edge, pointcloud_surf); odomEstimation_->initMapWithPoints(pointcloud_edge, pointcloud_surf);
}else{ }else{

1
corelib/src/odometry/OdometryLOAM.cpp
View File

@@ -56,7 +56,6 @@ OdometryLOAM::OdometryLOAM(const ParametersMap & parameters) :
float mapResolution = Parameters::defaultOdomLOAMResolution(); float mapResolution = Parameters::defaultOdomLOAMResolution();
Parameters::parse(parameters, Parameters::kOdomLOAMSensor(), velodyneType); Parameters::parse(parameters, Parameters::kOdomLOAMSensor(), velodyneType);
Parameters::parse(parameters, Parameters::kOdomLOAMScanPeriod(), scanPeriod_); Parameters::parse(parameters, Parameters::kOdomLOAMScanPeriod(), scanPeriod_);
UASSERT(scanPeriod_>0.0f);
Parameters::parse(parameters, Parameters::kOdomLOAMResolution(), mapResolution); Parameters::parse(parameters, Parameters::kOdomLOAMResolution(), mapResolution);
UASSERT(mapResolution>0.0f); UASSERT(mapResolution>0.0f);
Parameters::parse(parameters, Parameters::kOdomLOAMLinVar(), linVar_); Parameters::parse(parameters, Parameters::kOdomLOAMLinVar(), linVar_);