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

DbViewer: fixed occupancy grid color options. Reprocess: added grid total time including rtabmap stats

Browse Source
This commit is contained in:
matlabbe
2018-02-18 21:32:19 -05:00
parent 00559ce8d6
commit 37cbf79b4c
2 changed files with 8 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
guilib/src/DatabaseViewer.cpp
View File

@@ -5183,24 +5183,24 @@ void DatabaseViewer::updateOctomapView()
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), Qt::red);
occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
occupancyGridViewer_->setCloudPointSize("octomap_obstacles", 5);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud, *ground, *groundCloud);
occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), Qt::green);
occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
occupancyGridViewer_->setCloudPointSize("octomap_ground", 5);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), Qt::red);
occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
occupancyGridViewer_->setCloudPointSize("octomap_obstacles", 5);
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, *ground, *groundCloud);
occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), Qt::green);
occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
occupancyGridViewer_->setCloudPointSize("octomap_ground", 5);
}
@@ -5208,7 +5208,7 @@ void DatabaseViewer::updateOctomapView()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, *empty, *emptyCloud);
occupancyGridViewer_->addCloud("octomap_empty", emptyCloud, Transform::getIdentity(), Qt::white);
occupancyGridViewer_->addCloud("octomap_empty", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
occupancyGridViewer_->setCloudOpacity("octomap_empty", 0.5);
occupancyGridViewer_->setCloudPointSize("octomap_empty", 5);
}

4
tools/Reprocess/main.cpp
View File

@@ -222,6 +222,7 @@ int main(int argc, char * argv[])
printf("High variance detected, triggering a new map...\n");
rtabmap.triggerNewMap();
}
UTimer t;
if(!rtabmap.process(data, info.odomPose, info.odomCovariance, info.odomVelocity, globalMapStats))
{
printf("Failed processing node %d.\n", data.id());
@@ -230,13 +231,13 @@ int main(int argc, char * argv[])
else if(assemble2dMap || assemble3dMap || assemble2dOctoMap || assemble3dOctoMap)
{
globalMapStats.clear();
double timeRtabmap = t.ticks();
double timeUpdateInit = 0.0;
double timeUpdateGrid = 0.0;
#ifdef RTABMAP_OCTOMAP
double timeUpdateOctoMap = 0.0;
#endif
const rtabmap::Statistics & stats = rtabmap.getStatistics();
UTimer t;
if(stats.poses().size() && stats.getSignatures().size())
{
int id = stats.poses().rbegin()->first;
@@ -302,6 +303,7 @@ int main(int argc, char * argv[])
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapProjection/ms"), timePub2dOctoMap*1000.0f));
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctomapToCloud/ms"), timePub3dOctoMap*1000.0f));
#endif
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeUpdateOctoMap+timePub2dOctoMap+timePub3dOctoMap+timeRtabmap)*1000.0f));
}
}