diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h index 2942fba5..66529736 100644 --- a/guilib/include/rtabmap/gui/CloudViewer.h +++ b/guilib/include/rtabmap/gui/CloudViewer.h @@ -142,6 +142,10 @@ public: bool addOctomap(const OctoMap * octomap, unsigned int treeDepth = 0, bool showEdges = true, bool lightingOn = false); void removeOctomap(); + bool addTextureMesh ( + const pcl::TextureMesh &mesh, + const std::string &id = "texture", + int viewport = 0); bool addOccupancyGridMap( const cv::Mat & map8U, float resolution, // cell size diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index 5ca0a849..c69694c2 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -52,6 +52,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#if PCL_VERSION_COMPARE(<, 1, 7, 2) +#include +#include +#include +#include +#include +#include +#include +#endif #ifdef RTABMAP_OCTOMAP #include @@ -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 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 jpeg_reader = vtkSmartPointer::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 bmp_reader = vtkSmartPointer::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 pnm_reader = vtkSmartPointer::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 png_reader = vtkSmartPointer::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 tiff_reader = vtkSmartPointer::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 poly_points = vtkSmartPointer::New (); + vtkSmartPointer colors = vtkSmartPointer::New (); + bool has_color = false; + vtkSmartPointer transformation = vtkSmartPointer::New (); + if ((pcl::getFieldIndex(mesh.cloud, "rgba") != -1) || + (pcl::getFieldIndex(mesh.cloud, "rgb") != -1)) + { + pcl::PointCloud 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::Ptr cloud (new pcl::PointCloud ()); + 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 polys = vtkSmartPointer::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 polydata = vtkSmartPointer::New(); + polydata->SetPolys (polys); + polydata->SetPoints (poly_points); + if (has_color) + polydata->GetPointData()->SetScalars(colors); + + vtkSmartPointer mapper = vtkSmartPointer::New (); +#if VTK_MAJOR_VERSION < 6 + mapper->SetInput (polydata); +#else + mapper->SetInputData (polydata); +#endif + + vtkSmartPointer actor = vtkSmartPointer::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 (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 texture = vtkSmartPointer::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 coordinates = vtkSmartPointer::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 texture = vtkSmartPointer::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 coordinates = vtkSmartPointer::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( diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index da895ccd..ccb0fcb7 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -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); } } } diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 0570394f..784010b6 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -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()); + } } } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index ca3689ba..c6c89b1b 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -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); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 86208bea..aaee99e8 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -86,7 +86,7 @@ QFrame::Raised - 13 + 3 @@ -1514,7 +1514,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - When the Graph View is visible or if the "Show in 3D map view" 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. + When the Graph View is visible or if the "Show in 3D map view" 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. true