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

CloudViewer: no need to save texture on disk before rendering TextureMesh (opencv image to vtk texture directly)

Browse Source
This commit is contained in:
matlabbe
2017-01-30 21:23:56 -05:00
parent 31d8ae08c8
commit f61569622a
8 changed files with 285 additions and 342 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/src/CMakeLists.txt
View File

@@ -221,6 +221,7 @@ IF(GTSAM_FOUND)
${INCLUDE_DIRS} ${INCLUDE_DIRS}
) )
ENDIF() ENDIF()
add_definitions("-DGTSAM_IMPORT_STATIC")
SET(LIBRARIES SET(LIBRARIES
${LIBRARIES} ${LIBRARIES}
gtsam gtsam

7
guilib/include/rtabmap/gui/CloudViewer.h
View File

@@ -134,16 +134,20 @@ public:
const pcl::PolygonMesh::Ptr & mesh, const pcl::PolygonMesh::Ptr & mesh,
const Transform & pose = Transform::getIdentity()); const Transform & pose = Transform::getIdentity());
// Only one texture per mesh is supported!
bool addCloudTextureMesh( bool addCloudTextureMesh(
const std::string & id, const std::string & id,
const pcl::TextureMesh::Ptr & textureMesh, const pcl::TextureMesh::Ptr & textureMesh,
const cv::Mat & texture,
const Transform & pose = Transform::getIdentity()); const Transform & pose = Transform::getIdentity());
bool addOctomap(const OctoMap * octomap, unsigned int treeDepth = 0); bool addOctomap(const OctoMap * octomap, unsigned int treeDepth = 0);
void removeOctomap(); void removeOctomap();
// Only one texture per mesh is supported!
bool addTextureMesh ( bool addTextureMesh (
const pcl::TextureMesh &mesh, const pcl::TextureMesh &mesh,
const cv::Mat & texture,
const std::string &id = "texture", const std::string &id = "texture",
int viewport = 0); int viewport = 0);
bool addOccupancyGridMap( bool addOccupancyGridMap(
@@ -269,8 +273,6 @@ public:
void setGridCellCount(unsigned int count); void setGridCellCount(unsigned int count);
void setGridCellSize(float size); void setGridCellSize(float size);
void setWorkingDirectory(const QString & path) {_workingDirectory = path;}
public slots: public slots:
void setDefaultBackgroundColor(const QColor & color); void setDefaultBackgroundColor(const QColor & color);
void setBackgroundColor(const QColor & color); void setBackgroundColor(const QColor & color);
@@ -335,7 +337,6 @@ private:
Transform _lastPose; Transform _lastPose;
std::list<std::string> _gridLines; std::list<std::string> _gridLines;
QSet<Qt::Key> _keysPressed; QSet<Qt::Key> _keysPressed;
QString _workingDirectory;
QColor _defaultBgColor; QColor _defaultBgColor;
QColor _currentBgColor; QColor _currentBgColor;
bool _frontfaceCulling; bool _frontfaceCulling;

1
guilib/src/CMakeLists.txt
View File

@@ -96,6 +96,7 @@ SET(SRC_FILES
./ParametersToolBox.cpp ./ParametersToolBox.cpp
./DepthCalibrationDialog.cpp ./DepthCalibrationDialog.cpp
./3rdParty/QMultiComboBox.cpp ./3rdParty/QMultiComboBox.cpp
./opencv/vtkImageMatSource.cpp
${moc_srcs} ${moc_srcs}
${moc_uis} ${moc_uis}

237
guilib/src/CloudViewer.cpp
View File

@@ -59,6 +59,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <vtkPNGReader.h> #include <vtkPNGReader.h>
#include <vtkTIFFReader.h> #include <vtkTIFFReader.h>
#include <vtkOpenGLRenderWindow.h> #include <vtkOpenGLRenderWindow.h>
#include <opencv/vtkImageMatSource.h>
#ifdef RTABMAP_OCTOMAP #ifdef RTABMAP_OCTOMAP
#include <rtabmap/core/OctoMap.h> #include <rtabmap/core/OctoMap.h>
@@ -138,7 +139,6 @@ CloudViewer::CloudViewer(QWidget *parent) :
_gridCellSize(1), _gridCellSize(1),
_lastCameraOrientation(0,0,0), _lastCameraOrientation(0,0,0),
_lastCameraPose(0,0,0), _lastCameraPose(0,0,0),
_workingDirectory("."),
_defaultBgColor(Qt::black), _defaultBgColor(Qt::black),
_currentBgColor(Qt::black), _currentBgColor(Qt::black),
_frontfaceCulling(false), _frontfaceCulling(false),
@@ -638,6 +638,7 @@ bool CloudViewer::addCloudMesh(
bool CloudViewer::addCloudTextureMesh( bool CloudViewer::addCloudTextureMesh(
const std::string & id, const std::string & id,
const pcl::TextureMesh::Ptr & textureMesh, const pcl::TextureMesh::Ptr & textureMesh,
const cv::Mat & texture,
const Transform & pose) const Transform & pose)
{ {
if(_addedClouds.contains(id)) if(_addedClouds.contains(id))
@@ -646,7 +647,7 @@ bool CloudViewer::addCloudTextureMesh(
} }
UDEBUG("Adding %s", id.c_str()); UDEBUG("Adding %s", id.c_str());
if(this->addTextureMesh(*textureMesh, id)) if(this->addTextureMesh(*textureMesh, texture, id))
{ {
_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked()); _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked()); _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
@@ -769,133 +770,13 @@ void CloudViewer::removeOctomap()
#endif #endif
} }
// 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);
}
bool CloudViewer::addTextureMesh ( bool CloudViewer::addTextureMesh (
const pcl::TextureMesh &mesh, const pcl::TextureMesh &mesh,
const cv::Mat & image,
const std::string &id, const std::string &id,
int viewport) int viewport)
{ {
// Copied from PCL 1.8, modified to ignore vertex color // Copied from PCL 1.8, modified to ignore vertex color and accept only one material (loaded from memory instead of file)
pcl::visualization::CloudActorMap::iterator am_it = _visualizer->getCloudActorMap()->find (id); pcl::visualization::CloudActorMap::iterator am_it = _visualizer->getCloudActorMap()->find (id);
if (am_it != _visualizer->getCloudActorMap()->end ()) if (am_it != _visualizer->getCloudActorMap()->end ())
@@ -911,6 +792,11 @@ bool CloudViewer::addTextureMesh (
PCL_ERROR("[PCLVisualizer::addTextureMesh] No textures found!\n"); PCL_ERROR("[PCLVisualizer::addTextureMesh] No textures found!\n");
return (false); return (false);
} }
else if (mesh.tex_materials.size() > 1)
{
PCL_ERROR("[PCLVisualizer::addTextureMesh] only one material per mesh is supported!\n");
return (false);
}
// polygons are mapped to texture materials // polygons are mapped to texture materials
if (mesh.tex_materials.size () != mesh.tex_polygons.size ()) if (mesh.tex_materials.size () != mesh.tex_polygons.size ())
{ {
@@ -951,31 +837,7 @@ bool CloudViewer::addTextureMesh (
vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New (); vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
bool has_color = false; bool has_color = false;
vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New (); vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New ();
if (0)//(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::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::fromPCLPointCloud2 (mesh.cloud, *cloud); pcl::fromPCLPointCloud2 (mesh.cloud, *cloud);
// no points --> exit // no points --> exit
@@ -991,7 +853,6 @@ bool CloudViewer::addTextureMesh (
const pcl::PointXYZ &p = cloud->points[i]; const pcl::PointXYZ &p = cloud->points[i];
poly_points->InsertPoint (i, p.x, p.y, p.z); poly_points->InsertPoint (i, p.x, p.y, p.z);
} }
}
//create polys from polyMesh.tex_polygons //create polys from polyMesh.tex_polygons
vtkSmartPointer<vtkCellArray> polys = vtkSmartPointer<vtkCellArray>::New (); vtkSmartPointer<vtkCellArray> polys = vtkSmartPointer<vtkCellArray>::New ();
@@ -1023,68 +884,13 @@ bool CloudViewer::addTextureMesh (
vtkTextureUnitManager* tex_manager = vtkOpenGLRenderWindow::SafeDownCast (_visualizer->getRenderWindow())->GetTextureUnitManager (); vtkTextureUnitManager* tex_manager = vtkOpenGLRenderWindow::SafeDownCast (_visualizer->getRenderWindow())->GetTextureUnitManager ();
if (!tex_manager) if (!tex_manager)
return (false); 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 (); vtkSmartPointer<vtkTexture> texture = vtkSmartPointer<vtkTexture>::New ();
// fill vtkTexture from pcl::TexMaterial structure // fill vtkTexture from pcl::TexMaterial structure
if (textureFromTexMaterial (mesh.tex_materials[0], texture)) vtkSmartPointer<vtkImageMatSource> cvImageToVtk = vtkSmartPointer<vtkImageMatSource>::New();
PCL_WARN ("[PCLVisualizer::addTextureMesh] Failed to create vtkTexture from %s!\n", cvImageToVtk->SetImage(image);
mesh.tex_materials[0].tex_name.c_str ()); cvImageToVtk->Update();
texture->SetInputConnection(cvImageToVtk->GetOutputPort());
// set texture coordinates // set texture coordinates
vtkSmartPointer<vtkFloatArray> coordinates = vtkSmartPointer<vtkFloatArray>::New (); vtkSmartPointer<vtkFloatArray> coordinates = vtkSmartPointer<vtkFloatArray>::New ();
@@ -1099,7 +905,6 @@ bool CloudViewer::addTextureMesh (
polydata->GetPointData ()->SetTCoords(coordinates); polydata->GetPointData ()->SetTCoords(coordinates);
// apply texture // apply texture
actor->SetTexture (texture); actor->SetTexture (texture);
} // end of one texture
// set mapper // set mapper
actor->SetMapper (mapper); actor->SetMapper (mapper);
@@ -1175,10 +980,7 @@ bool CloudViewer::addOccupancyGridMap(
// default texture materials parameters // default texture materials parameters
pcl::TexMaterial material; pcl::TexMaterial material;
// hack, can we read from memory? material.tex_file = "";
std::string tmpPath = (_workingDirectory+"/.tmp_map.png").toStdString();
cv::imwrite(tmpPath, map8U);
material.tex_file = tmpPath;
mesh->tex_materials.push_back(material); mesh->tex_materials.push_back(material);
#if PCL_VERSION_COMPARE(>=, 1, 8, 0) #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
@@ -1192,11 +994,8 @@ bool CloudViewer::addOccupancyGridMap(
coordinates.push_back(Eigen::Vector2f(0,0)); coordinates.push_back(Eigen::Vector2f(0,0));
mesh->tex_coordinates.push_back(coordinates); mesh->tex_coordinates.push_back(coordinates);
this->addTextureMesh(*mesh, "map"); this->addTextureMesh(*mesh, map8U, "map");
setCloudOpacity("map", opacity); setCloudOpacity("map", opacity);
//removed tmp texture file
QFile::remove(tmpPath.c_str());
} }
return true; return true;
} }

115
guilib/src/ExportCloudsDialog.cpp
View File

@@ -621,7 +621,6 @@ void ExportCloudsDialog::viewClouds(
window->resize(QDesktopWidget().availableGeometry(this).size() * 0.7); window->resize(QDesktopWidget().availableGeometry(this).size() * 0.7);
CloudViewer * viewer = new CloudViewer(window); CloudViewer * viewer = new CloudViewer(window);
viewer->setCameraLockZ(false);
if(_ui->comboBox_pipeline->currentIndex() == 0) if(_ui->comboBox_pipeline->currentIndex() == 0)
{ {
viewer->setBackfaceCulling(true, false); viewer->setBackfaceCulling(true, false);
@@ -644,19 +643,16 @@ void ExportCloudsDialog::viewClouds(
if(textureMeshes.size()) if(textureMeshes.size())
{ {
QString prefix = "tmp_textures"; for (std::map<int, pcl::TextureMesh::Ptr>::iterator iter = textureMeshes.begin(); iter != textureMeshes.end(); ++iter)
removeDirRecursively(workingDirectory+QDir::separator()+prefix);
QDir(workingDirectory).mkdir(prefix);
for(std::map<int, pcl::TextureMesh::Ptr>::iterator iter = textureMeshes.begin(); iter!=textureMeshes.end(); ++iter)
{ {
_progressDialog->appendText(tr("Viewing the mesh %1 (%2 polygons)...").arg(iter->first).arg(iter->second->tex_polygons.size()?iter->second->tex_polygons[0].size():0)); _progressDialog->appendText(tr("Viewing the mesh %1 (%2 polygons)...").arg(iter->first).arg(iter->second->tex_polygons.size() ? iter->second->tex_polygons[0].size() : 0));
_progressDialog->incrementStep(); _progressDialog->incrementStep();
pcl::TextureMesh::Ptr mesh = iter->second; pcl::TextureMesh::Ptr mesh = iter->second;
// As CloudViewer is not supporting more than one texture per mesh, merge them all by default // As CloudViewer is not supporting more than one texture per mesh, merge them all by default
cv::Mat globalTexture; cv::Mat globalTexture;
if(mesh->tex_materials.size() > 1) if (mesh->tex_materials.size() > 1)
{ {
globalTexture = mergeTextures(*mesh, cachedSignatures); globalTexture = mergeTextures(*mesh, cachedSignatures);
} }
@@ -665,7 +661,7 @@ void ExportCloudsDialog::viewClouds(
// tex_coordinates should be linked to points, not // tex_coordinates should be linked to points, not
// polygon vertices. Points linked to multiple different TCoords (different textures) should // polygon vertices. Points linked to multiple different TCoords (different textures) should
// be duplicated. // be duplicated.
for(unsigned int t=0; t<mesh->tex_coordinates.size(); ++t) for (unsigned int t = 0; t < mesh->tex_coordinates.size(); ++t)
{ {
UASSERT(mesh->tex_polygons[t].size()); UASSERT(mesh->tex_polygons[t].size());
@@ -674,15 +670,16 @@ void ExportCloudsDialog::viewClouds(
// make a cloud with as many points than polygon vertices // make a cloud with as many points than polygon vertices
unsigned int nPoints = mesh->tex_coordinates[t].size(); unsigned int nPoints = mesh->tex_coordinates[t].size();
UASSERT(nPoints== mesh->tex_polygons[t].size()*mesh->tex_polygons[t][0].vertices.size()); // assuming polygon size is constant! UASSERT(nPoints == mesh->tex_polygons[t].size()*mesh->tex_polygons[t][0].vertices.size()); // assuming polygon size is constant!
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(nPoints); cloud->resize(nPoints);
unsigned int oi=0; unsigned int oi = 0;
for(unsigned int i=0; i<mesh->tex_polygons[t].size(); ++i) for (unsigned int i = 0; i < mesh->tex_polygons[t].size(); ++i)
{ {
pcl::Vertices & vertices = mesh->tex_polygons[t][i]; pcl::Vertices & vertices = mesh->tex_polygons[t][i];
for(unsigned int j=0; j<vertices.vertices.size(); ++j) for(unsigned int j=0; j<vertices.vertices.size(); ++j)
{ {
UASSERT(oi < cloud->size()); UASSERT(oi < cloud->size());
@@ -695,77 +692,23 @@ void ExportCloudsDialog::viewClouds(
pcl::toPCLPointCloud2(*cloud, mesh->cloud); pcl::toPCLPointCloud2(*cloud, mesh->cloud);
} }
// save tmp textures if (globalTexture.empty())
cv::Size imageSize;
for(unsigned int i=0;i<mesh->tex_materials.size(); ++i)
{ {
if(!mesh->tex_materials[i].tex_file.empty()) UASSERT(mesh->tex_materials.size()==1 &&
{ !mesh->tex_materials[0].tex_file.empty() &&
// absolute path uIsInteger(mesh->tex_materials[0].tex_file, false));
QString fullPath = workingDirectory+QDir::separator()+prefix+QDir::separator()+QString(mesh->tex_materials[i].tex_file.c_str())+_ui->comboBox_meshingTextureFormat->currentText();
if(!QFileInfo(fullPath).exists())
{
if(uIsInteger(mesh->tex_materials[i].tex_file, false))
{
int textureId = uStr2Int(mesh->tex_materials[i].tex_file);
UASSERT(cachedSignatures.contains(textureId) && !cachedSignatures.value(textureId).sensorData().imageCompressed().empty());
cv::Mat image;
cachedSignatures.value(textureId).sensorData().uncompressDataConst(&image, 0);
UASSERT(!image.empty());
imageSize = image.size();
if(_ui->groupBox_gain->isChecked() && _compensator && _compensator->getIndex(textureId) >= 0)
{
_compensator->apply(textureId, image);
}
if(!cv::imwrite(fullPath.toStdString(), image)) int textureId = uStr2Int(mesh->tex_materials[0].tex_file);
{ UASSERT(cachedSignatures.contains(textureId) && !cachedSignatures.value(textureId).sensorData().imageCompressed().empty());
_progressDialog->appendText(tr("Failed saving texture \"%1\" to \"%2\".") cachedSignatures.value(textureId).sensorData().uncompressDataConst(&globalTexture, 0);
.arg(mesh->tex_materials[i].tex_file.c_str()).arg(fullPath), Qt::darkRed); UASSERT(!globalTexture.empty());
_progressDialog->setAutoClose(false); if (_ui->groupBox_gain->isChecked() && _compensator && _compensator->getIndex(textureId) >= 0)
} {
} _compensator->apply(textureId, globalTexture);
else if(imageSize.height && imageSize.width)
{
// make a blank texture
cv::Mat image = cv::Mat::ones(imageSize, CV_8UC1)*255;
cv::imwrite(fullPath.toStdString(), image);
}
else if(!globalTexture.empty())
{
if(!cv::imwrite(fullPath.toStdString(), globalTexture))
{
_progressDialog->appendText(tr("Failed saving texture \"%1\" to \"%2\".")
.arg(mesh->tex_materials[i].tex_file.c_str()).arg(fullPath), Qt::darkRed);
_progressDialog->setAutoClose(false);
}
}
else
{
UWARN("Ignored texture %s (no image size set yet)", mesh->tex_materials[i].tex_file.c_str());
}
}
mesh->tex_materials[i].tex_file=fullPath.toStdString();
} }
} }
bool isRGB = false; viewer->addCloudTextureMesh(uFormat("mesh%d",iter->first), mesh, globalTexture, iter->first>0?poses.at(iter->first):Transform::getIdentity());
for(unsigned int i=0; i<mesh->cloud.fields.size(); ++i)
{
if(mesh->cloud.fields[i].name.compare("rgb") == 0)
{
isRGB=true;
break;
}
}
if(isRGB)
{
viewer->addCloudTextureMesh(uFormat("mesh%d",iter->first), mesh, iter->first>0?poses.at(iter->first):Transform::getIdentity());
}
else
{
viewer->addCloudTextureMesh(uFormat("mesh%d",iter->first), mesh, iter->first>0?poses.at(iter->first):Transform::getIdentity());
}
_progressDialog->appendText(tr("Viewing the mesh %1 (%2 polygons)... done.").arg(iter->first).arg(mesh->tex_polygons.size()?mesh->tex_polygons[0].size():0)); _progressDialog->appendText(tr("Viewing the mesh %1 (%2 polygons)... done.").arg(iter->first).arg(mesh->tex_polygons.size()?mesh->tex_polygons[0].size():0));
QApplication::processEvents(); QApplication::processEvents();
} }
@@ -1038,7 +981,7 @@ bool ExportCloudsDialog::getExportedClouds(
.arg(assembledCloud->size()) .arg(assembledCloud->size())
.arg(_ui->doubleSpinBox_voxelSize_assembled->value())); .arg(_ui->doubleSpinBox_voxelSize_assembled->value()));
QApplication::processEvents(); QApplication::processEvents();
int before = assembledCloud->size(); unsigned int before = assembledCloud->size();
assembledCloud = util3d::voxelize( assembledCloud = util3d::voxelize(
assembledCloud, assembledCloud,
_ui->doubleSpinBox_voxelSize_assembled->value()); _ui->doubleSpinBox_voxelSize_assembled->value());
@@ -1221,7 +1164,7 @@ bool ExportCloudsDialog::getExportedClouds(
std::vector<std::set<int> > neighbors; std::vector<std::set<int> > neighbors;
std::vector<std::set<int> > vertexToPolygons; std::vector<std::set<int> > vertexToPolygons;
util3d::createPolygonIndexes(polygons, util3d::createPolygonIndexes(polygons,
iter->second->size(), (int)iter->second->size(),
neighbors, neighbors,
vertexToPolygons); vertexToPolygons);
std::list<std::list<int> > clusters = util3d::clusterPolygons( std::list<std::list<int> > clusters = util3d::clusterPolygons(
@@ -1237,7 +1180,7 @@ bool ExportCloudsDialog::getExportedClouds(
} }
} }
filteredPolygons.resize(oi); filteredPolygons.resize(oi);
int before = polygons.size(); int before = (int)polygons.size();
polygons = filteredPolygons; polygons = filteredPolygons;
if(oi == 0) if(oi == 0)
@@ -1575,7 +1518,7 @@ bool ExportCloudsDialog::getExportedClouds(
} }
} }
filteredPolygons.resize(oi); filteredPolygons.resize(oi);
int before = mesh->polygons.size(); int before = (int)mesh->polygons.size();
mesh->polygons = filteredPolygons; mesh->polygons = filteredPolygons;
_progressDialog->appendText(tr("Filtered %1 polygons.").arg(before-oi)); _progressDialog->appendText(tr("Filtered %1 polygons.").arg(before-oi));
@@ -1665,7 +1608,7 @@ bool ExportCloudsDialog::getExportedClouds(
textureMesh->tex_coordinates.resize(1); textureMesh->tex_coordinates.resize(1);
//tex_coordinates should be linked to polygon vertices //tex_coordinates should be linked to polygon vertices
int polygonSize = textureMesh->tex_polygons[0][0].vertices.size(); int polygonSize = (int)textureMesh->tex_polygons[0][0].vertices.size();
textureMesh->tex_coordinates[0].resize(polygonSize*textureMesh->tex_polygons[0].size()); textureMesh->tex_coordinates[0].resize(polygonSize*textureMesh->tex_polygons[0].size());
for(unsigned int i=0; i<textureMesh->tex_polygons[0].size(); ++i) for(unsigned int i=0; i<textureMesh->tex_polygons[0].size(); ++i)
{ {
@@ -1723,7 +1666,7 @@ bool ExportCloudsDialog::getExportedClouds(
QApplication::processEvents(); QApplication::processEvents();
// concatenate all polygons // concatenate all polygons
int totalSize = 0; unsigned int totalSize = 0;
for(unsigned int t=0; t<textureMesh->tex_polygons.size(); ++t) for(unsigned int t=0; t<textureMesh->tex_polygons.size(); ++t)
{ {
totalSize+=textureMesh->tex_polygons[t].size(); totalSize+=textureMesh->tex_polygons[t].size();
@@ -1742,7 +1685,7 @@ bool ExportCloudsDialog::getExportedClouds(
std::vector<std::set<int> > neighbors; std::vector<std::set<int> > neighbors;
std::vector<std::set<int> > vertexToPolygons; std::vector<std::set<int> > vertexToPolygons;
util3d::createPolygonIndexes(allPolygons, util3d::createPolygonIndexes(allPolygons,
textureMesh->cloud.data.size()/textureMesh->cloud.point_step, (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step,
neighbors, neighbors,
vertexToPolygons); vertexToPolygons);
std::list<std::list<int> > clusters = util3d::clusterPolygons( std::list<std::list<int> > clusters = util3d::clusterPolygons(
@@ -2066,8 +2009,8 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
} }
clouds.insert(std::make_pair(iter->first, std::make_pair(cloud, indices))); clouds.insert(std::make_pair(iter->first, std::make_pair(cloud, indices)));
points = cloud->size(); points = (int)cloud->size();
totalIndices = indices->size(); totalIndices = (int)indices->size();
} }
} }
else else

38
guilib/src/MainWindow.cpp
View File

@@ -519,7 +519,6 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->statsToolBox->setNewFigureMaxItems(50); _ui->statsToolBox->setNewFigureMaxItems(50);
_ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory()); _ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
_ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory()); _ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
_cloudViewer->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
_cloudViewer->setBackfaceCulling(true, false); _cloudViewer->setBackfaceCulling(true, false);
_preferencesDialog->loadWidgetState(_cloudViewer); _preferencesDialog->loadWidgetState(_cloudViewer);
@@ -1017,25 +1016,10 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
mesh_material.tex_illum = 1; mesh_material.tex_illum = 1;
mesh_material.tex_name = "material_odom"; mesh_material.tex_name = "material_odom";
mesh_material.tex_file = "";
QDir dir(_preferencesDialog->getWorkingDirectory());
ExportCloudsDialog::removeDirRecursively(_preferencesDialog->getWorkingDirectory()+QDir::separator()+"tmp_textures");
dir.mkdir("tmp_textures");
std::string tmpDirectory = dir.filePath("tmp_textures").toStdString();
mesh_material.tex_file = uFormat("%s/%s.png", tmpDirectory.c_str(), "texture_odom");
if(!cv::imwrite(mesh_material.tex_file, odom.data().imageRaw()))
{
UERROR("Cannot save texture of image odom");
}
else
{
UINFO("Saved temporary texture: \"%s\"", mesh_material.tex_file.c_str());
}
textureMesh->tex_materials.push_back(mesh_material); textureMesh->tex_materials.push_back(mesh_material);
if(!_cloudViewer->addCloudTextureMesh("cloudOdom", textureMesh, _odometryCorrection)) if(!_cloudViewer->addCloudTextureMesh("cloudOdom", textureMesh, odom.data().imageRaw(), _odometryCorrection))
{ {
UERROR("Adding cloudOdom to viewer failed!"); UERROR("Adding cloudOdom to viewer failed!");
} }
@@ -2613,28 +2597,15 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
tex_name << "material_" << nodeId; tex_name << "material_" << nodeId;
tex_name >> mesh_material.tex_name; tex_name >> mesh_material.tex_name;
QDir dir(_preferencesDialog->getWorkingDirectory()); mesh_material.tex_file = "";
ExportCloudsDialog::removeDirRecursively(_preferencesDialog->getWorkingDirectory()+QDir::separator()+"tmp_textures");
dir.mkdir("tmp_textures");
std::string tmpDirectory = dir.filePath("tmp_textures").toStdString();
mesh_material.tex_file = uFormat("%s/%s%d.png", tmpDirectory.c_str(), "texture_", nodeId);
if(_preferencesDialog->gainCompensation() && compensator.getIndex(0) >= 0) if(_preferencesDialog->gainCompensation() && compensator.getIndex(0) >= 0)
{ {
compensator.apply(0, image); compensator.apply(0, image);
} }
if(!cv::imwrite(mesh_material.tex_file, image))
{
UERROR("Cannot save texture of image %d", nodeId);
}
else
{
UINFO("Saved temporary texture: \"%s\"", mesh_material.tex_file.c_str());
}
textureMesh->tex_materials.push_back(mesh_material); textureMesh->tex_materials.push_back(mesh_material);
if(!_cloudViewer->addCloudTextureMesh(cloudName, textureMesh, pose)) if(!_cloudViewer->addCloudTextureMesh(cloudName, textureMesh, image, pose))
{ {
UERROR("Adding texture mesh %d to viewer failed!", nodeId); UERROR("Adding texture mesh %d to viewer failed!", nodeId);
} }
@@ -3528,7 +3499,6 @@ void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters, bo
{ {
_ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory()); _ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
_ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory()); _ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
_cloudViewer->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
} }
if(_state != kIdle && parametersModified.size()) if(_state != kIdle && parametersModified.size())

147
guilib/src/opencv/vtkImageMatSource.cpp Normal file
View File

@@ -0,0 +1,147 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
// Authors:
// * Anatoly Baksheev, Itseez Inc. myname.mysurname <> mycompany.com
//
//M*/
#include "vtkImageMatSource.h"
#include <vtkImageData.h>
#include <vtkInformation.h>
#include <vtkInformationVector.h>
#include <vtkStreamingDemandDrivenPipeline.h>
#include <vtkObjectFactory.h>
namespace rtabmap {
vtkStandardNewMacro(vtkImageMatSource);
}
rtabmap::vtkImageMatSource::vtkImageMatSource()
{
this->SetNumberOfInputPorts(0);
this->ImageData = vtkSmartPointer<vtkImageData>::New();
}
int rtabmap::vtkImageMatSource::RequestInformation(vtkInformation *, vtkInformationVector**, vtkInformationVector *outputVector)
{
vtkInformation* outInfo = outputVector->GetInformationObject(0);
outInfo->Set(vtkStreamingDemandDrivenPipeline::WHOLE_EXTENT(), this->ImageData->GetExtent(), 6);
outInfo->Set(vtkDataObject::SPACING(), 1.0, 1.0, 1.0);
outInfo->Set(vtkDataObject::ORIGIN(), 0.0, 0.0, 0.0);
vtkDataObject::SetPointDataActiveScalarInfo(outInfo, this->ImageData->GetScalarType(), this->ImageData->GetNumberOfScalarComponents());
return 1;
}
int rtabmap::vtkImageMatSource::RequestData(vtkInformation*, vtkInformationVector**, vtkInformationVector *outputVector)
{
vtkInformation *outInfo = outputVector->GetInformationObject(0);
vtkImageData *output = vtkImageData::SafeDownCast(outInfo->Get(vtkDataObject::DATA_OBJECT()) );
output->ShallowCopy(this->ImageData);
return 1;
}
void rtabmap::vtkImageMatSource::SetImage(cv::InputArray _image)
{
CV_Assert(_image.depth() == CV_8U && (_image.channels() == 1 || _image.channels() == 3 || _image.channels() == 4));
cv::Mat image = _image.getMat();
this->ImageData->SetDimensions(image.cols, image.rows, 1);
#if VTK_MAJOR_VERSION <= 5
this->ImageData->SetNumberOfScalarComponents(image.channels());
this->ImageData->SetScalarTypeToUnsignedChar();
this->ImageData->AllocateScalars();
#else
this->ImageData->AllocateScalars(VTK_UNSIGNED_CHAR, image.channels());
#endif
switch(image.channels())
{
case 1: copyGrayImage(image, this->ImageData); break;
case 3: copyRGBImage (image, this->ImageData); break;
case 4: copyRGBAImage(image, this->ImageData); break;
}
this->ImageData->Modified();
}
void rtabmap::vtkImageMatSource::copyGrayImage(const cv::Mat &source, vtkSmartPointer<vtkImageData> output)
{
unsigned char* dptr = reinterpret_cast<unsigned char*>(output->GetScalarPointer());
size_t elem_step = output->GetIncrements()[1]/sizeof(unsigned char);
for (int y = 0; y < source.rows; ++y)
{
unsigned char* drow = dptr + elem_step * y;
const unsigned char *srow = source.ptr<unsigned char>(source.rows-(y+1)); // vertical flip for texturing
for (int x = 0; x < source.cols; ++x)
drow[x] = *srow++;
}
}
void rtabmap::vtkImageMatSource::copyRGBImage(const cv::Mat &source, vtkSmartPointer<vtkImageData> output)
{
cv::Vec3b* dptr = reinterpret_cast<cv::Vec3b*>(output->GetScalarPointer());
size_t elem_step = output->GetIncrements()[1]/sizeof(cv::Vec3b);
for (int y = 0; y < source.rows; ++y)
{
cv::Vec3b* drow = dptr + elem_step * y;
const unsigned char *srow = source.ptr<unsigned char>(source.rows - (y + 1)); // vertical flip for texturing
for (int x = 0; x < source.cols; ++x, srow += source.channels())
drow[x] = cv::Vec3b(srow[2], srow[1], srow[0]);
}
}
void rtabmap::vtkImageMatSource::copyRGBAImage(const cv::Mat &source, vtkSmartPointer<vtkImageData> output)
{
cv::Vec4b* dptr = reinterpret_cast<cv::Vec4b*>(output->GetScalarPointer());
size_t elem_step = output->GetIncrements()[1]/sizeof(cv::Vec4b);
for (int y = 0; y < source.rows; ++y)
{
cv::Vec4b* drow = dptr + elem_step * y;
const unsigned char *srow = source.ptr<unsigned char>(source.rows - (y + 1)); // vertical flip for texturing
for (int x = 0; x < source.cols; ++x, srow += source.channels())
drow[x] = cv::Vec4b(srow[2], srow[1], srow[0], srow[3]);
}
}

81
guilib/src/opencv/vtkImageMatSource.h Normal file
View File

@@ -0,0 +1,81 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
// Authors:
// * Anatoly Baksheev, Itseez Inc. myname.mysurname <> mycompany.com
//
//M*/
#ifndef RTABMAP__vtkImageMatSource_h
#define RTABMAP__vtkImageMatSource_h
#include <opencv2/core/core.hpp>
#include <vtkImageAlgorithm.h>
#include <vtkSmartPointer.h>
namespace rtabmap
{
class vtkImageMatSource : public vtkImageAlgorithm
{
public:
static vtkImageMatSource *New();
vtkTypeMacro(vtkImageMatSource, vtkImageAlgorithm);
void SetImage(cv::InputArray image);
protected:
vtkImageMatSource();
~vtkImageMatSource() {}
vtkSmartPointer<vtkImageData> ImageData;
int RequestInformation(vtkInformation*, vtkInformationVector**, vtkInformationVector*);
int RequestData(vtkInformation*, vtkInformationVector**, vtkInformationVector*);
private:
vtkImageMatSource(const vtkImageMatSource&); // Not implemented.
void operator=(const vtkImageMatSource&); // Not implemented.
static void copyGrayImage(const cv::Mat &source, vtkSmartPointer<vtkImageData> output);
static void copyRGBImage(const cv::Mat &source, vtkSmartPointer<vtkImageData> output);
static void copyRGBAImage(const cv::Mat &source, vtkSmartPointer<vtkImageData> output);
};
}
#endif