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

FLOAM: added some debug logs

Browse Source
This commit is contained in:
matlabbe
2021-09-24 11:32:25 -04:00
parent 5f65618d40
commit bccc5b13af
3 changed files with 14 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Parameters.cpp
+6
View File
@@ -824,6 +824,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
#else
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
#endif
str = "With FLOAM:";
#ifdef RTABMAP_FLOAM
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
#else
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
#endif
str = "With FOVIS:";
#ifdef RTABMAP_FOVIS
corelib/src/odometry/OdometryFLOAM.cpp
+7 -1
View File
@@ -114,6 +114,7 @@ Transform OdometryFLOAM::computeTransform(
Transform t;
#ifdef RTABMAP_FLOAM
UTimer timer;
UTimer timerTotal;
if(data.laserScanRaw().isEmpty())
{
@@ -131,16 +132,20 @@ Transform OdometryFLOAM::computeTransform(
{
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudInPtr = util3d::laserScanToPointCloudI(data.laserScanRaw(), data.laserScanRaw().localTransform());
UDEBUG("Scan conversion: %fs", timer.ticks());
pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud_edge(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud_surf(new pcl::PointCloud<pcl::PointXYZI>());
laserProcessing_->featureExtraction(laserCloudInPtr,pointcloud_edge,pointcloud_surf);
UDEBUG("Feature extraction: %fs", timer.ticks());
if(this->framesProcessed() == 0){
odomEstimation_->initMapWithPoints(pointcloud_edge, pointcloud_surf);
}else{
odomEstimation_->updatePointsToMap(pointcloud_edge, pointcloud_surf);
}
UDEBUG("Update: %fs", timer.ticks());
Transform pose = Transform::fromEigen3d(odomEstimation_->odom);
@@ -174,6 +179,7 @@ Transform OdometryFLOAM::computeTransform(
odomEstimation_->getMap(localMap);
info->localScanMapSize = localMap->size();
info->localScanMap = LaserScan(util3d::laserScanFromPointCloud(*localMap), 0, data.laserScanRaw().rangeMax());
UDEBUG("Fill info data: %fs", timer.ticks());
}
}
}
@@ -183,7 +189,7 @@ Transform OdometryFLOAM::computeTransform(
UWARN("FLOAM failed to register the latest scan, odometry should be reset.");
}
}
UINFO("Odom update time = %fs, lost=%s", timer.elapsed(), lost_?"true":"false");
UINFO("Odom update time = %fs, lost=%s", timerTotal.elapsed(), lost_?"true":"false");
#else
UERROR("RTAB-Map is not built with FLOAM support! Select another odometry approach.");
tools/Export/main.cpp
+1 -1
View File
@@ -105,7 +105,7 @@ void showUsage()
" --decimation # Depth image decimation before creating the clouds (default 4, 1 with --scan).\n"
" --voxel # Voxel size of the created clouds (default 0.01 m, 0 m with --scan).\n"
" --noise_radius # Noise filtering search radius (default 0, 0=disabled).\n"
" --noise_k # Noise filtering minimum neighbors in search radius (default 5, 0=disabled)."
" --noise_k # Noise filtering minimum neighbors in search radius (default 5, 0=disabled).\n"
" --color_radius # Radius used to colorize polygons (default 0.05 m, 0 m with --scan). Set 0 for nearest color.\n"
" --scan Use laser scan for the point cloud.\n"
" --save_in_db Save resulting assembled point cloud or mesh in the database.\n"