i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 08:47:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

CloudViewer: added occupancy grid support for PCL < 1.7.2

Browse Source
This commit is contained in:
matlabbe
2016-08-31 12:43:54 -04:00
parent 4e027a6515
commit 8d31b6140d
6 changed files with 430 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
guilib/src/CloudViewer.cpp
+383 -12
View File
@@ -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(
guilib/src/DatabaseViewer.cpp
+21 -6
View File
@@ -595,12 +595,24 @@ bool DatabaseViewer::openDatabase(const QString & path)
iter->second.compare(jter->second) != 0 &&
iter->first.compare(Parameters::kRtabmapWorkingDirectory()) != 0)
{
differentParameters.insert(*iter);
QString msg = tr("Parameter \"%1\": database=\"%2\" Preferences=\"%3\"")
.arg(iter->first.c_str())
.arg(iter->second.c_str())
.arg(jter->second.c_str());
UWARN(msg.toStdString().c_str());
bool different = true;
if(Parameters::getType(iter->first).compare("double") ==0 ||
Parameters::getType(iter->first).compare("float") == 0)
{
if(uStr2Double(iter->second) == uStr2Double(jter->second))
{
different = false;
}
}
if(different)
{
differentParameters.insert(*iter);
QString msg = tr("Parameter \"%1\": database=\"%2\" Preferences=\"%3\"")
.arg(iter->first.c_str())
.arg(iter->second.c_str())
.arg(jter->second.c_str());
UWARN(msg.toStdString().c_str());
}
}
}
@@ -615,10 +627,13 @@ bool DatabaseViewer::openDatabase(const QString & path)
QMessageBox::Yes);
if(r == QMessageBox::Yes)
{
QStringList str;
for(rtabmap::ParametersMap::const_iterator iter = differentParameters.begin(); iter!=differentParameters.end(); ++iter)
{
ui_->parameters_toolbox->updateParameter(iter->first, iter->second);
str.push_back(iter->first.c_str());
}
notifyParametersChanged(str);
}
}
}
guilib/src/MainWindow.cpp
+19 -7
View File
@@ -3752,13 +3752,25 @@ void MainWindow::openDatabase(const QString & path)
iter->second.compare(jter->second) != 0 &&
iter->first.compare(Parameters::kRtabmapWorkingDirectory()) != 0)
{
differentParameters.insert(*iter);
QString msg = tr("Parameter \"%1\": database=\"%2\" Preferences=\"%3\"")
.arg(iter->first.c_str())
.arg(iter->second.c_str())
.arg(jter->second.c_str());
_ui->widget_console->appendMsg(msg);
UWARN(msg.toStdString().c_str());
bool different = true;
if(Parameters::getType(iter->first).compare("double") ==0 ||
Parameters::getType(iter->first).compare("float") == 0)
{
if(uStr2Double(iter->second) == uStr2Double(jter->second))
{
different = false;
}
}
if(different)
{
differentParameters.insert(*iter);
QString msg = tr("Parameter \"%1\": database=\"%2\" Preferences=\"%3\"")
.arg(iter->first.c_str())
.arg(iter->second.c_str())
.arg(jter->second.c_str());
_ui->widget_console->appendMsg(msg);
UWARN(msg.toStdString().c_str());
}
}
}
guilib/src/PreferencesDialog.cpp
+1 -8
View File
@@ -146,13 +146,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->reextract_nn->removeItem(4);
}
#if PCL_VERSION_COMPARE(<, 1, 7, 2)
_ui->checkBox_map_shown->setChecked(false);
_ui->checkBox_map_shown->setEnabled(false);
_ui->label_map_shown->setText(_ui->label_map_shown->text() + " (Disabled, PCL >=1.7.2 required)");
_ui->label_map_shown->setEnabled(false);
#endif
#ifndef RTABMAP_OCTOMAP
_ui->groupBox_octomap->setChecked(false);
_ui->groupBox_octomap->setEnabled(false);
@@ -1242,7 +1235,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->checkBox_map_erode->setChecked(false);
_ui->checkBox_map_incremental->setChecked(false);
_ui->doubleSpinBox_map_footprintRadius->setValue(0);
_ui->doubleSpinBox_map_opacity->setValue(0.75);
_ui->doubleSpinBox_map_opacity->setValue(1);
_ui->groupBox_octomap->setChecked(false);
_ui->spinBox_octomap_treeDepth->setValue(16);
guilib/src/ui/preferencesDialog.ui
+2 -2
View File
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>13</number>
<number>3</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -1514,7 +1514,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
<item>
<widget class="QLabel" name="label_167">
<property name="text">
<string>When the Graph View is visible or if the &quot;Show in 3D map view&quot; below is checked, the grid map is assembled using occupancy grids saved in each node. If nodes don't have local occupancy grid created, use the Database Viewer tool to generate them.</string>
<string>When the Graph View is visible or if the &quot;Show in 3D map view&quot; below is checked, the grid map is assembled using occupancy grids saved in each node. See Occupancy Grid (advanced) panel for parameters used to create the local occupancy grids. If nodes don't have local occupancy grid created, use the Database Viewer tool to generate them.</string>
</property>
<property name="wordWrap">
<bool>true</bool>