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

Added Octomap visualization and export options

Browse Source
This commit is contained in:
matlabbe
2016-07-15 17:46:13 -04:00
parent c24079884d
commit b4b7b6f455
13 changed files with 737 additions and 397 deletions
Download Patch File Download Diff File Expand all files Collapse all files

120
guilib/src/CloudViewer.cpp
View File

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/gui/CloudViewer.h"
#include <rtabmap/core/Version.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UMath.h>
@@ -47,6 +48,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <vtkCamera.h>
#include <vtkRenderWindow.h>
#include <vtkCubeSource.h>
#include <vtkGlyph3D.h>
#include <vtkGlyph3DMapper.h>
#include <vtkLookupTable.h>
#ifdef RTABMAP_OCTOMAP
#include <rtabmap/core/OctoMap.h>
#endif
namespace rtabmap {
@@ -123,7 +132,8 @@ CloudViewer::CloudViewer(QWidget *parent) :
_currentBgColor(Qt::black),
_backfaceCulling(false),
_frontfaceCulling(false),
_renderingRate(5.0)
_renderingRate(5.0),
_octomapActor(0)
{
UDEBUG("");
this->setMinimumSize(200, 200);
@@ -166,6 +176,7 @@ CloudViewer::~CloudViewer()
UDEBUG("");
this->clear();
delete _visualizer;
UDEBUG("");
}
void CloudViewer::clear()
@@ -177,6 +188,8 @@ void CloudViewer::clear()
this->removeAllFrustums();
this->removeAllTexts();
this->clearTrajectory();
this->removeOccupancyGridMap();
this->removeOctomap();
this->addOrUpdateCoordinate("reference", Transform::getIdentity(), 0.2);
}
@@ -639,6 +652,111 @@ bool CloudViewer::addCloudTextureMesh(
return false;
}
bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth, bool showEdges, bool lightingOn)
{
UDEBUG("");
#ifdef RTABMAP_OCTOMAP
UASSERT(octomap!=0);
pcl::IndicesPtr obstacles(new std::vector<int>);
if(treeDepth > octomap->octree()->getTreeDepth())
{
UWARN("Tree depth requested (%d) is deeper than the "
"actual maximum tree depth of %d. Using maximum depth.",
(int)treeDepth, (int)octomap->octree()->getTreeDepth());
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(treeDepth, obstacles.get());
if(obstacles->size())
{
//get the renderer of the visualizer object
vtkRenderer *renderer = _visualizer->getRenderWindow()->GetRenderers()->GetFirstRenderer();
if(_octomapActor)
{
renderer->RemoveActor(_octomapActor);
_octomapActor = 0;
}
//vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
//colors->SetName("colors");
//colors->SetNumberOfComponents(3);
vtkSmartPointer<vtkFloatArray> colors = vtkSmartPointer<vtkFloatArray>::New();
colors->SetName("colors");
colors->SetNumberOfValues(obstacles->size());
vtkSmartPointer<vtkLookupTable> lut = vtkSmartPointer<vtkLookupTable>::New();
lut->SetNumberOfTableValues(obstacles->size());
lut->Build();
// Create points
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
double s = octomap->octree()->getNodeSize(treeDepth) / 2.0;
for (unsigned int i = 0; i < obstacles->size(); i++)
{
points->InsertNextPoint(
cloud->at(obstacles->at(i)).x,
cloud->at(obstacles->at(i)).y,
cloud->at(obstacles->at(i)).z);
colors->InsertValue(i,i);
lut->SetTableValue(i,
double(cloud->at(obstacles->at(i)).r) / 255.0,
double(cloud->at(obstacles->at(i)).g) / 255.0,
double(cloud->at(obstacles->at(i)).b) / 255.0);
}
// Combine into a polydata
vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
polydata->SetPoints(points);
polydata->GetPointData()->SetScalars(colors);
// Create anything you want here, we will use a cube for the demo.
vtkSmartPointer<vtkCubeSource> cubeSource = vtkSmartPointer<vtkCubeSource>::New();
cubeSource->SetBounds(-s, s, -s, s, -s, s);
vtkSmartPointer<vtkGlyph3DMapper> mapper = vtkSmartPointer<vtkGlyph3DMapper>::New();
mapper->SetSourceConnection(cubeSource->GetOutputPort());
#if VTK_MAJOR_VERSION <= 5
mapper->SetInputConnection(polydata->GetProducerPort());
#else
mapper->SetInputData(polydata);
#endif
mapper->SetScalarRange(0, obstacles->size() - 1);
mapper->SetLookupTable(lut);
mapper->ScalingOff();
mapper->Update();
vtkSmartPointer<vtkActor> octomapActor = vtkSmartPointer<vtkActor>::New();
octomapActor->SetMapper(mapper);
octomapActor->GetProperty()->SetRepresentationToSurface();
octomapActor->GetProperty()->SetEdgeVisibility(showEdges);
octomapActor->GetProperty()->SetLighting(lightingOn);
renderer->AddActor(octomapActor);
_octomapActor = octomapActor.GetPointer();
return true;
#endif
}
return false;
}
void CloudViewer::removeOctomap()
{
UDEBUG("");
#ifdef RTABMAP_OCTOMAP
if(_octomapActor)
{
vtkRenderer *renderer = _visualizer->getRenderWindow()->GetRenderers()->GetFirstRenderer();
renderer->RemoveActor(_octomapActor);
_octomapActor = 0;
}
#endif
}
bool CloudViewer::addOccupancyGridMap(
const cv::Mat & map8U,
float resolution, // cell size