|
|
|
|
@@ -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(
|
|
|
|
|
|