mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-03 01:50:24 +08:00
CloudViewer: added occupancy grid support for PCL < 1.7.2
This commit is contained in:
matlabbe
@@ -52,6 +52,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <vtkGlyph3D.h>
|
||||
#include <vtkGlyph3DMapper.h>
|
||||
#include <vtkLookupTable.h>
|
||||
#if PCL_VERSION_COMPARE(<, 1, 7, 2)
|
||||
#include <vtkTextureUnitManager.h>
|
||||
#include <vtkJPEGReader.h>
|
||||
#include <vtkBMPReader.h>
|
||||
#include <vtkPNMReader.h>
|
||||
#include <vtkPNGReader.h>
|
||||
#include <vtkTIFFReader.h>
|
||||
#include <vtkOpenGLRenderWindow.h>
|
||||
#endif
|
||||
|
||||
#ifdef RTABMAP_OCTOMAP
|
||||
#include <rtabmap/core/OctoMap.h>
|
||||
@@ -757,6 +766,375 @@ void CloudViewer::removeOctomap()
|
||||
#endif
|
||||
}
|
||||
|
||||
#if PCL_VERSION_COMPARE(<, 1, 7, 2)
|
||||
// Copied from PCL 1.8
|
||||
int textureFromTexMaterial (const pcl::TexMaterial& tex_mat,
|
||||
vtkTexture* vtk_tex)
|
||||
{
|
||||
if (tex_mat.tex_file == "")
|
||||
{
|
||||
PCL_WARN ("[PCLVisualizer::textureFromTexMaterial] No texture file given for material %s!\n",
|
||||
tex_mat.tex_name.c_str ());
|
||||
return (-1);
|
||||
}
|
||||
|
||||
boost::filesystem::path full_path (tex_mat.tex_file.c_str ());
|
||||
if (!boost::filesystem::exists (full_path))
|
||||
{
|
||||
boost::filesystem::path parent_dir = full_path.parent_path ();
|
||||
std::string upper_filename = tex_mat.tex_file;
|
||||
boost::to_upper (upper_filename);
|
||||
std::string real_name = "";
|
||||
|
||||
try
|
||||
{
|
||||
if (!boost::filesystem::exists (parent_dir))
|
||||
{
|
||||
PCL_WARN ("[PCLVisualizer::textureFromTexMaterial] Parent directory '%s' doesn't exist!\n",
|
||||
parent_dir.string ().c_str ());
|
||||
return (-1);
|
||||
}
|
||||
|
||||
if (!boost::filesystem::is_directory (parent_dir))
|
||||
{
|
||||
PCL_WARN ("[PCLVisualizer::textureFromTexMaterial] Parent '%s' is not a directory !\n",
|
||||
parent_dir.string ().c_str ());
|
||||
return (-1);
|
||||
}
|
||||
|
||||
typedef std::vector<boost::filesystem::path> paths_vector;
|
||||
paths_vector paths;
|
||||
std::copy (boost::filesystem::directory_iterator (parent_dir),
|
||||
boost::filesystem::directory_iterator (),
|
||||
back_inserter (paths));
|
||||
|
||||
for (paths_vector::const_iterator it = paths.begin (); it != paths.end (); ++it)
|
||||
{
|
||||
if (boost::filesystem::is_regular_file (*it))
|
||||
{
|
||||
std::string name = it->string ();
|
||||
boost::to_upper (name);
|
||||
if (name == upper_filename)
|
||||
{
|
||||
real_name = it->string ();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Check texture file existence
|
||||
if (real_name == "")
|
||||
{
|
||||
PCL_WARN ("[PCLVisualizer::textureFromTexMaterial] Can not find texture file %s!\n",
|
||||
tex_mat.tex_file.c_str ());
|
||||
return (-1);
|
||||
}
|
||||
}
|
||||
catch (const boost::filesystem::filesystem_error& ex)
|
||||
{
|
||||
|
||||
PCL_WARN ("[PCLVisualizer::textureFromTexMaterial] Error %s when looking for file %s\n!",
|
||||
ex.what (), tex_mat.tex_file.c_str ());
|
||||
return (-1);
|
||||
}
|
||||
|
||||
//Save the real path
|
||||
full_path = real_name.c_str ();
|
||||
}
|
||||
|
||||
std::string extension = full_path.extension ().string ();
|
||||
//!!! nizar 20131206 : The list is far from being exhaustive I am afraid.
|
||||
if ((extension == ".jpg") || (extension == ".JPG"))
|
||||
{
|
||||
vtkSmartPointer<vtkJPEGReader> jpeg_reader = vtkSmartPointer<vtkJPEGReader>::New ();
|
||||
jpeg_reader->SetFileName (full_path.string ().c_str ());
|
||||
jpeg_reader->Update ();
|
||||
vtk_tex->SetInputConnection (jpeg_reader->GetOutputPort ());
|
||||
}
|
||||
else if ((extension == ".bmp") || (extension == ".BMP"))
|
||||
{
|
||||
vtkSmartPointer<vtkBMPReader> bmp_reader = vtkSmartPointer<vtkBMPReader>::New ();
|
||||
bmp_reader->SetFileName (full_path.string ().c_str ());
|
||||
bmp_reader->Update ();
|
||||
vtk_tex->SetInputConnection (bmp_reader->GetOutputPort ());
|
||||
}
|
||||
else if ((extension == ".pnm") || (extension == ".PNM"))
|
||||
{
|
||||
vtkSmartPointer<vtkPNMReader> pnm_reader = vtkSmartPointer<vtkPNMReader>::New ();
|
||||
pnm_reader->SetFileName (full_path.string ().c_str ());
|
||||
pnm_reader->Update ();
|
||||
vtk_tex->SetInputConnection (pnm_reader->GetOutputPort ());
|
||||
}
|
||||
else if ((extension == ".png") || (extension == ".PNG"))
|
||||
{
|
||||
vtkSmartPointer<vtkPNGReader> png_reader = vtkSmartPointer<vtkPNGReader>::New ();
|
||||
png_reader->SetFileName (full_path.string ().c_str ());
|
||||
png_reader->Update ();
|
||||
vtk_tex->SetInputConnection (png_reader->GetOutputPort ());
|
||||
}
|
||||
else if ((extension == ".tiff") || (extension == ".TIFF"))
|
||||
{
|
||||
vtkSmartPointer<vtkTIFFReader> tiff_reader = vtkSmartPointer<vtkTIFFReader>::New ();
|
||||
tiff_reader->SetFileName (full_path.string ().c_str ());
|
||||
tiff_reader->Update ();
|
||||
vtk_tex->SetInputConnection (tiff_reader->GetOutputPort ());
|
||||
}
|
||||
else
|
||||
{
|
||||
PCL_WARN ("[PCLVisualizer::textureFromTexMaterial] Unhandled image %s for material %s!\n",
|
||||
full_path.c_str (), tex_mat.tex_name.c_str ());
|
||||
return (-1);
|
||||
}
|
||||
|
||||
return (0);
|
||||
}
|
||||
#endif
|
||||
|
||||
bool CloudViewer::addTextureMesh (
|
||||
const pcl::TextureMesh &mesh,
|
||||
const std::string &id,
|
||||
int viewport)
|
||||
{
|
||||
#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
|
||||
return addTextureMesh(mesh, id, viewport);
|
||||
#else
|
||||
// Copied from PCL 1.8
|
||||
|
||||
pcl::visualization::CloudActorMap::iterator am_it = _visualizer->getCloudActorMap()->find (id);
|
||||
if (am_it != _visualizer->getCloudActorMap()->end ())
|
||||
{
|
||||
PCL_ERROR ("[PCLVisualizer::addTextureMesh] A shape with id <%s> already exists!"
|
||||
" Please choose a different id and retry.\n",
|
||||
id.c_str ());
|
||||
return (false);
|
||||
}
|
||||
// no texture materials --> exit
|
||||
if (mesh.tex_materials.size () == 0)
|
||||
{
|
||||
PCL_ERROR("[PCLVisualizer::addTextureMesh] No textures found!\n");
|
||||
return (false);
|
||||
}
|
||||
// polygons are mapped to texture materials
|
||||
if (mesh.tex_materials.size () != mesh.tex_polygons.size ())
|
||||
{
|
||||
PCL_ERROR("[PCLVisualizer::addTextureMesh] Materials number %lu differs from polygons number %lu!\n",
|
||||
mesh.tex_materials.size (), mesh.tex_polygons.size ());
|
||||
return (false);
|
||||
}
|
||||
// each texture material should have its coordinates set
|
||||
if (mesh.tex_materials.size () != mesh.tex_coordinates.size ())
|
||||
{
|
||||
PCL_ERROR("[PCLVisualizer::addTextureMesh] Coordinates number %lu differs from materials number %lu!\n",
|
||||
mesh.tex_coordinates.size (), mesh.tex_materials.size ());
|
||||
return (false);
|
||||
}
|
||||
// total number of vertices
|
||||
std::size_t nb_vertices = 0;
|
||||
for (std::size_t i = 0; i < mesh.tex_polygons.size (); ++i)
|
||||
nb_vertices+= mesh.tex_polygons[i].size ();
|
||||
// no vertices --> exit
|
||||
if (nb_vertices == 0)
|
||||
{
|
||||
PCL_ERROR("[PCLVisualizer::addTextureMesh] No vertices found!\n");
|
||||
return (false);
|
||||
}
|
||||
// total number of coordinates
|
||||
std::size_t nb_coordinates = 0;
|
||||
for (std::size_t i = 0; i < mesh.tex_coordinates.size (); ++i)
|
||||
nb_coordinates+= mesh.tex_coordinates[i].size ();
|
||||
// no texture coordinates --> exit
|
||||
if (nb_coordinates == 0)
|
||||
{
|
||||
PCL_ERROR("[PCLVisualizer::addTextureMesh] No textures coordinates found!\n");
|
||||
return (false);
|
||||
}
|
||||
|
||||
// Create points from mesh.cloud
|
||||
vtkSmartPointer<vtkPoints> poly_points = vtkSmartPointer<vtkPoints>::New ();
|
||||
vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
|
||||
bool has_color = false;
|
||||
vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New ();
|
||||
if ((pcl::getFieldIndex(mesh.cloud, "rgba") != -1) ||
|
||||
(pcl::getFieldIndex(mesh.cloud, "rgb") != -1))
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZRGB> cloud;
|
||||
pcl::fromPCLPointCloud2(mesh.cloud, cloud);
|
||||
if (cloud.points.size () == 0)
|
||||
{
|
||||
PCL_ERROR("[PCLVisualizer::addTextureMesh] Cloud is empty!\n");
|
||||
return (false);
|
||||
}
|
||||
pcl::visualization::PCLVisualizer::convertToVtkMatrix (cloud.sensor_origin_, cloud.sensor_orientation_, transformation);
|
||||
has_color = true;
|
||||
colors->SetNumberOfComponents (3);
|
||||
colors->SetName ("Colors");
|
||||
poly_points->SetNumberOfPoints (cloud.size ());
|
||||
for (std::size_t i = 0; i < cloud.points.size (); ++i)
|
||||
{
|
||||
const pcl::PointXYZRGB &p = cloud.points[i];
|
||||
poly_points->InsertPoint (i, p.x, p.y, p.z);
|
||||
const unsigned char color[3] = {p.r, p.g, p.b};
|
||||
colors->InsertNextTupleValue(color);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
|
||||
pcl::fromPCLPointCloud2 (mesh.cloud, *cloud);
|
||||
// no points --> exit
|
||||
if (cloud->points.size () == 0)
|
||||
{
|
||||
PCL_ERROR("[PCLVisualizer::addTextureMesh] Cloud is empty!\n");
|
||||
return (false);
|
||||
}
|
||||
pcl::visualization::PCLVisualizer::convertToVtkMatrix (cloud->sensor_origin_, cloud->sensor_orientation_, transformation);
|
||||
poly_points->SetNumberOfPoints (cloud->points.size ());
|
||||
for (std::size_t i = 0; i < cloud->points.size (); ++i)
|
||||
{
|
||||
const pcl::PointXYZ &p = cloud->points[i];
|
||||
poly_points->InsertPoint (i, p.x, p.y, p.z);
|
||||
}
|
||||
}
|
||||
|
||||
//create polys from polyMesh.tex_polygons
|
||||
vtkSmartPointer<vtkCellArray> polys = vtkSmartPointer<vtkCellArray>::New ();
|
||||
for (std::size_t i = 0; i < mesh.tex_polygons.size (); i++)
|
||||
{
|
||||
for (std::size_t j = 0; j < mesh.tex_polygons[i].size (); j++)
|
||||
{
|
||||
std::size_t n_points = mesh.tex_polygons[i][j].vertices.size ();
|
||||
polys->InsertNextCell (int (n_points));
|
||||
for (std::size_t k = 0; k < n_points; k++)
|
||||
polys->InsertCellPoint (mesh.tex_polygons[i][j].vertices[k]);
|
||||
}
|
||||
}
|
||||
|
||||
vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
|
||||
polydata->SetPolys (polys);
|
||||
polydata->SetPoints (poly_points);
|
||||
if (has_color)
|
||||
polydata->GetPointData()->SetScalars(colors);
|
||||
|
||||
vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
|
||||
#if VTK_MAJOR_VERSION < 6
|
||||
mapper->SetInput (polydata);
|
||||
#else
|
||||
mapper->SetInputData (polydata);
|
||||
#endif
|
||||
|
||||
vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New ();
|
||||
vtkTextureUnitManager* tex_manager = vtkOpenGLRenderWindow::SafeDownCast (_visualizer->getRenderWindow())->GetTextureUnitManager ();
|
||||
if (!tex_manager)
|
||||
return (false);
|
||||
// Check if hardware support multi texture
|
||||
int texture_units = tex_manager->GetNumberOfTextureUnits ();
|
||||
if ((mesh.tex_materials.size () > 1) && (texture_units > 1))
|
||||
{
|
||||
if (texture_units < (int)mesh.tex_materials.size ())
|
||||
PCL_WARN ("[PCLVisualizer::addTextureMesh] GPU texture units %d < mesh textures %d!\n",
|
||||
texture_units, mesh.tex_materials.size ());
|
||||
// Load textures
|
||||
std::size_t last_tex_id = std::min (static_cast<int> (mesh.tex_materials.size ()), texture_units);
|
||||
int tu = vtkProperty::VTK_TEXTURE_UNIT_0;
|
||||
std::size_t tex_id = 0;
|
||||
while (tex_id < last_tex_id)
|
||||
{
|
||||
vtkSmartPointer<vtkTexture> texture = vtkSmartPointer<vtkTexture>::New ();
|
||||
if (textureFromTexMaterial (mesh.tex_materials[tex_id], texture))
|
||||
{
|
||||
PCL_WARN ("[PCLVisualizer::addTextureMesh] Failed to load texture %s, skipping!\n",
|
||||
mesh.tex_materials[tex_id].tex_name.c_str ());
|
||||
continue;
|
||||
}
|
||||
// the first texture is in REPLACE mode others are in ADD mode
|
||||
if (tex_id == 0)
|
||||
texture->SetBlendingMode(vtkTexture::VTK_TEXTURE_BLENDING_MODE_REPLACE);
|
||||
else
|
||||
texture->SetBlendingMode(vtkTexture::VTK_TEXTURE_BLENDING_MODE_ADD);
|
||||
// add a texture coordinates array per texture
|
||||
vtkSmartPointer<vtkFloatArray> coordinates = vtkSmartPointer<vtkFloatArray>::New ();
|
||||
coordinates->SetNumberOfComponents (2);
|
||||
std::stringstream ss; ss << "TCoords" << tex_id;
|
||||
std::string this_coordinates_name = ss.str ();
|
||||
coordinates->SetName (this_coordinates_name.c_str ());
|
||||
|
||||
for (std::size_t t = 0 ; t < mesh.tex_coordinates.size (); ++t)
|
||||
if (t == tex_id)
|
||||
for (std::size_t tc = 0; tc < mesh.tex_coordinates[t].size (); ++tc)
|
||||
coordinates->InsertNextTuple2 ((double)mesh.tex_coordinates[t][tc][0],
|
||||
(double)mesh.tex_coordinates[t][tc][1]);
|
||||
else
|
||||
for (std::size_t tc = 0; tc < mesh.tex_coordinates[t].size (); ++tc)
|
||||
coordinates->InsertNextTuple2 (-1.0, -1.0);
|
||||
|
||||
mapper->MapDataArrayToMultiTextureAttribute(tu,
|
||||
this_coordinates_name.c_str (),
|
||||
vtkDataObject::FIELD_ASSOCIATION_POINTS);
|
||||
polydata->GetPointData ()->AddArray (coordinates);
|
||||
actor->GetProperty ()->SetTexture(tu, texture);
|
||||
++tex_id;
|
||||
++tu;
|
||||
}
|
||||
} // end of multi texturing
|
||||
else
|
||||
{
|
||||
if ((mesh.tex_materials.size () > 1) && (texture_units < 2))
|
||||
PCL_WARN ("[PCLVisualizer::addTextureMesh] Your GPU doesn't support multi texturing. "
|
||||
"Will use first one only!\n");
|
||||
|
||||
vtkSmartPointer<vtkTexture> texture = vtkSmartPointer<vtkTexture>::New ();
|
||||
// fill vtkTexture from pcl::TexMaterial structure
|
||||
if (textureFromTexMaterial (mesh.tex_materials[0], texture))
|
||||
PCL_WARN ("[PCLVisualizer::addTextureMesh] Failed to create vtkTexture from %s!\n",
|
||||
mesh.tex_materials[0].tex_name.c_str ());
|
||||
|
||||
// set texture coordinates
|
||||
vtkSmartPointer<vtkFloatArray> coordinates = vtkSmartPointer<vtkFloatArray>::New ();
|
||||
coordinates->SetNumberOfComponents (2);
|
||||
coordinates->SetNumberOfTuples (mesh.tex_coordinates[0].size ());
|
||||
for (std::size_t tc = 0; tc < mesh.tex_coordinates[0].size (); ++tc)
|
||||
{
|
||||
const Eigen::Vector2f &uv = mesh.tex_coordinates[0][tc];
|
||||
coordinates->SetTuple2 (tc, (double)uv[0], (double)uv[1]);
|
||||
}
|
||||
coordinates->SetName ("TCoords");
|
||||
polydata->GetPointData ()->SetTCoords(coordinates);
|
||||
// apply texture
|
||||
actor->SetTexture (texture);
|
||||
} // end of one texture
|
||||
|
||||
// set mapper
|
||||
actor->SetMapper (mapper);
|
||||
|
||||
|
||||
//_visualizer->addActorToRenderer (actor, viewport);
|
||||
// Add it to all renderers
|
||||
_visualizer->getRendererCollection()->InitTraversal ();
|
||||
vtkRenderer* renderer = NULL;
|
||||
int i = 0;
|
||||
while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL)
|
||||
{
|
||||
// Should we add the actor to all renderers?
|
||||
if (viewport == 0)
|
||||
{
|
||||
renderer->AddActor (actor);
|
||||
}
|
||||
else if (viewport == i) // add the actor only to the specified viewport
|
||||
{
|
||||
renderer->AddActor (actor);
|
||||
}
|
||||
++i;
|
||||
}
|
||||
|
||||
// Save the pointer/ID pair to the global actor map
|
||||
(*_visualizer->getCloudActorMap())[id].actor = actor;
|
||||
|
||||
// Save the viewpoint transformation matrix to the global actor map
|
||||
(*_visualizer->getCloudActorMap())[id].viewpoint_transformation_ = transformation;
|
||||
|
||||
return (true);
|
||||
#endif
|
||||
}
|
||||
|
||||
bool CloudViewer::addOccupancyGridMap(
|
||||
const cv::Mat & map8U,
|
||||
float resolution, // cell size
|
||||
@@ -764,14 +1142,13 @@ bool CloudViewer::addOccupancyGridMap(
|
||||
float yMin,
|
||||
float opacity)
|
||||
{
|
||||
#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
|
||||
UASSERT(map8U.channels() == 1 && map8U.type() == CV_8U);
|
||||
|
||||
float xSize = float(map8U.cols) * resolution;
|
||||
float ySize = float(map8U.rows) * resolution;
|
||||
|
||||
UDEBUG("resolution=%f, xSize=%f, ySize=%f, xMin=%f, yMin=%f", resolution, xSize, ySize, xMin, yMin);
|
||||
if(_visualizer->getShapeActorMap()->find("map") == _visualizer->getShapeActorMap()->end())
|
||||
if(_visualizer->getCloudActorMap()->find("map") == _visualizer->getCloudActorMap()->end())
|
||||
{
|
||||
_visualizer->removeShape("map");
|
||||
}
|
||||
@@ -813,28 +1190,22 @@ bool CloudViewer::addOccupancyGridMap(
|
||||
coordinates.push_back(Eigen::Vector2f(0,0));
|
||||
mesh->tex_coordinates.push_back(coordinates);
|
||||
|
||||
_visualizer->addTextureMesh(*mesh, "map");
|
||||
this->addTextureMesh(*mesh, "map");
|
||||
_visualizer->getCloudActorMap()->find("map")->second.actor->GetProperty()->LightingOff();
|
||||
setCloudOpacity("map", 0.7);
|
||||
setCloudOpacity("map", opacity);
|
||||
|
||||
//removed tmp texture file
|
||||
QFile::remove(tmpPath.c_str());
|
||||
}
|
||||
return true;
|
||||
#else
|
||||
// not implemented on lower version of PCL
|
||||
return false;
|
||||
#endif
|
||||
}
|
||||
|
||||
void CloudViewer::removeOccupancyGridMap()
|
||||
{
|
||||
#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
|
||||
if(_visualizer->getShapeActorMap()->find("map") == _visualizer->getShapeActorMap()->end())
|
||||
if(_visualizer->getCloudActorMap()->find("map") != _visualizer->getCloudActorMap()->end())
|
||||
{
|
||||
_visualizer->removeShape("map");
|
||||
_visualizer->removePointCloud("map");
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void CloudViewer::addOrUpdateCoordinate(
|
||||
|
||||
Reference in New Issue
Block a user