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

Moved ExportCloudsDialog::mergeTextures() method in util3d_surface module

Browse Source
This commit is contained in:
matlabbe
2017-06-13 19:20:09 -04:00
parent 29a48aba66
commit 811afa1171
9 changed files with 1308 additions and 1058 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/ProgressState.h
+3
View File
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef CORELIB_INCLUDE_RTABMAP_CORE_PROGRESSSTATE_H_
#define CORELIB_INCLUDE_RTABMAP_CORE_PROGRESSSTATE_H_
#include <rtabmap/utilite/ULogger.h>
class ProgressState
{
@@ -35,6 +36,8 @@ public:
ProgressState():canceled_(false){}
virtual bool callback(const std::string & msg) const
{
if(!msg.empty())
UDEBUG("msg=%s", msg.c_str());
return true;
}
virtual ~ProgressState(){}
corelib/include/rtabmap/core/impl/util3d_surface.hpp
+236
View File
@@ -8,6 +8,9 @@
#ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_
#define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_
#include <pcl/search/kdtree.h>
#include <rtabmap/utilite/UConversion.h>
namespace rtabmap {
namespace util3d {
@@ -42,6 +45,239 @@ std::vector<pcl::Vertices> normalizePolygonsSide(
return output;
}
template<typename pointRGBT>
void denseMeshPostProcessing(
pcl::PolygonMeshPtr & mesh,
bool hasColors,
float meshDecimationFactor,
int maximumPolygons,
const typename pcl::PointCloud<pointRGBT>::Ptr & cloud,
float transferColorRadius,
bool coloredOutput,
bool cleanMesh,
int minClusterSize,
ProgressState * progressState)
{
if(maximumPolygons > 0)
{
double factor = 1.0-double(maximumPolygons)/double(mesh->polygons.size());
if(factor > meshDecimationFactor)
{
meshDecimationFactor = factor;
}
}
if(meshDecimationFactor > 0.0)
{
unsigned int count = mesh->polygons.size();
if(progressState) progressState->callback(uFormat("Mesh decimation (factor=%f) from %d polygons...",meshDecimationFactor, (int)count));
mesh = util3d::meshDecimation(mesh, (float)meshDecimationFactor);
if(progressState) progressState->callback(uFormat("Mesh decimated (factor=%f) from %d to %d polygons", meshDecimationFactor, (int)count, (int)mesh->polygons.size()));
if(count < mesh->polygons.size())
{
if(progressState) progressState->callback(uFormat("Decimated mesh has more polygons than before!"));
}
hasColors = false;
}
if(cloud.get()!=0 &&
!hasColors &&
transferColorRadius >= 0.0 &&
coloredOutput)
{
if(progressState) progressState->callback(uFormat("Transferring color from point cloud to mesh..."));
// transfer color from point cloud to mesh
typename pcl::search::KdTree<pointRGBT>::Ptr tree (new pcl::search::KdTree<pointRGBT>(true));
tree->setInputCloud(cloud);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr coloredCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromPCLPointCloud2(mesh->cloud, *coloredCloud);
std::vector<bool> coloredPts(coloredCloud->size());
for(unsigned int i=0; i<coloredCloud->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
pointRGBT pt;
pt.x = coloredCloud->at(i).x;
pt.y = coloredCloud->at(i).y;
pt.z = coloredCloud->at(i).z;
if(transferColorRadius > 0.0)
{
tree->radiusSearch(pt, transferColorRadius, kIndices, kDistances);
}
else
{
tree->nearestKSearch(pt, 1, kIndices, kDistances);
}
if(kIndices.size())
{
//compute average color
int r=0;
int g=0;
int b=0;
int a=0;
for(unsigned int j=0; j<kIndices.size(); ++j)
{
r+=(int)cloud->at(kIndices[j]).r;
g+=(int)cloud->at(kIndices[j]).g;
b+=(int)cloud->at(kIndices[j]).b;
a+=(int)cloud->at(kIndices[j]).a;
}
coloredCloud->at(i).r = r/kIndices.size();
coloredCloud->at(i).g = g/kIndices.size();
coloredCloud->at(i).b = b/kIndices.size();
coloredCloud->at(i).a = a/kIndices.size();
coloredPts.at(i) = true;
}
else
{
//white
coloredCloud->at(i).r = coloredCloud->at(i).g = coloredCloud->at(i).b = 255;
coloredPts.at(i) = false;
}
}
pcl::toPCLPointCloud2(*coloredCloud, mesh->cloud);
// remove polygons with no color
if(cleanMesh)
{
std::vector<pcl::Vertices> filteredPolygons(mesh->polygons.size());
int oi=0;
for(unsigned int i=0; i<mesh->polygons.size(); ++i)
{
bool coloredPolygon = true;
for(unsigned int j=0; j<mesh->polygons[i].vertices.size(); ++j)
{
if(!coloredPts.at(mesh->polygons[i].vertices[j]))
{
coloredPolygon = false;
break;
}
}
if(coloredPolygon)
{
filteredPolygons[oi++] = mesh->polygons[i];
}
}
filteredPolygons.resize(oi);
mesh->polygons = filteredPolygons;
}
}
else if(cloud.get()!=0 &&
!hasColors &&
transferColorRadius > 0.0 &&
cleanMesh &&
!coloredOutput)
{
if(progressState) progressState->callback(uFormat("Removing polygons too far from the cloud..."));
// transfer color from point cloud to mesh
typename pcl::search::KdTree<pointRGBT>::Ptr tree (new pcl::search::KdTree<pointRGBT>(true));
tree->setInputCloud(cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr optimizedCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(mesh->cloud, *optimizedCloud);
std::vector<bool> closePts(optimizedCloud->size());
for(unsigned int i=0; i<optimizedCloud->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
pointRGBT pt;
pt.x = optimizedCloud->at(i).x;
pt.y = optimizedCloud->at(i).y;
pt.z = optimizedCloud->at(i).z;
tree->radiusSearch(pt, transferColorRadius, kIndices, kDistances);
if(kIndices.size())
{
closePts.at(i) = true;
}
else
{
closePts.at(i) = false;
}
}
// remove far polygons
std::vector<pcl::Vertices> filteredPolygons(mesh->polygons.size());
int oi=0;
for(unsigned int i=0; i<mesh->polygons.size(); ++i)
{
bool keepPolygon = true;
for(unsigned int j=0; j<mesh->polygons[i].vertices.size(); ++j)
{
if(!closePts.at(mesh->polygons[i].vertices[j]))
{
keepPolygon = false;
break;
}
}
if(keepPolygon)
{
filteredPolygons[oi++] = mesh->polygons[i];
}
}
filteredPolygons.resize(oi);
mesh->polygons = filteredPolygons;
}
if(minClusterSize && coloredOutput && !cleanMesh)
{
if(progressState) progressState->callback(uFormat("Filter small polygon clusters..."));
// filter polygons
std::vector<std::set<int> > neighbors;
std::vector<std::set<int> > vertexToPolygons;
util3d::createPolygonIndexes(mesh->polygons,
mesh->cloud.height*mesh->cloud.width,
neighbors,
vertexToPolygons);
std::list<std::list<int> > clusters = util3d::clusterPolygons(
neighbors,
minClusterSize<0?0:minClusterSize);
std::vector<pcl::Vertices> filteredPolygons(mesh->polygons.size());
if(minClusterSize < 0)
{
// only keep the biggest cluster
std::list<std::list<int> >::iterator biggestClusterIndex = clusters.end();
unsigned int biggestClusterSize = 0;
for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
{
if(iter->size() > biggestClusterSize)
{
biggestClusterIndex = iter;
biggestClusterSize = iter->size();
}
}
if(biggestClusterIndex != clusters.end())
{
int oi=0;
for(std::list<int>::iterator jter=biggestClusterIndex->begin(); jter!=biggestClusterIndex->end(); ++jter)
{
filteredPolygons[oi++] = mesh->polygons.at(*jter);
}
filteredPolygons.resize(oi);
}
}
else
{
int oi=0;
for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
{
for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
{
filteredPolygons[oi++] = mesh->polygons.at(*jter);
}
}
filteredPolygons.resize(oi);
}
int before = (int)mesh->polygons.size();
mesh->polygons = filteredPolygons;
if(progressState) progressState->callback(uFormat("Filtered %1 polygons.", before-(int)mesh->polygons.size()));
}
}
}
}
corelib/include/rtabmap/core/util3d.h
-6
View File
@@ -260,12 +260,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP concatenateClouds(
const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds);
pcl::TextureMesh::Ptr RTABMAP_EXP concatenateTextureMeshes(
const std::list<pcl::TextureMesh::Ptr> & meshes);
void RTABMAP_EXP concatenateTextureMaterials(
pcl::TextureMesh & mesh, const cv::Size & imageSize, int textureSize, int maxTextures, float & scale, std::vector<bool> * materialsKept=0);
/**
* @brief Concatenate a vector of indices to a single vector.
*
corelib/include/rtabmap/core/util3d_surface.h
+46
View File
@@ -44,6 +44,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap
{
class Memory;
class DBDriver;
namespace util3d
{
@@ -158,6 +161,36 @@ pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
const ProgressState * state = 0,
std::vector<std::map<int, pcl::PointXY> > * vertexToPixels = 0);
pcl::TextureMesh::Ptr RTABMAP_EXP concatenateTextureMeshes(
const std::list<pcl::TextureMesh::Ptr> & meshes);
void RTABMAP_EXP concatenateTextureMaterials(
pcl::TextureMesh & mesh, const cv::Size & imageSize, int textureSize, int maxTextures, float & scale, std::vector<bool> * materialsKept=0);
/*
* Merge all textures in the mesh into "textureCount" textures of size "textureSize".
* @return merged textures corresponding to new materials set in TextureMesh
*/
std::vector<cv::Mat> RTABMAP_EXP mergeTextures(
pcl::TextureMesh & mesh,
const std::map<int, cv::Mat> & images, // raw or compressed, can be empty if memory or dbDriver should be used
const std::map<int, std::vector<CameraModel> > & calibrations, // Should match images
const Memory * memory = 0, // Should be set if images are not set
const DBDriver * dbDriver = 0, // Should be set if images and memory are not set
int textureSize = 4096,
int textureCount = 1,
const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels = std::vector<std::map<int, pcl::PointXY> >(), // needed for parameters below
bool gainCompensation = true,
float gainBeta = 10.0f,
bool gainRGB = true, //Do gain compensation on each channel
bool blending = true,
int blendingDecimation = 0, //0=auto depending on projected polygon size and texture size
int brightnessContrastRatioLow = 0, //0=disabled, values between 0 and 100
int brightnessContrastRatioHigh = 0, //0=disabled, values between 0 and 100
bool exposureFusion = false); //Exposure fusion can be used only with OpenCV3
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int normalKSearch = 20,
@@ -230,6 +263,19 @@ std::vector<pcl::Vertices> normalizePolygonsSide(
const std::vector<pcl::Vertices> & polygons,
const pcl::PointXYZ & viewPoint = pcl::PointXYZ(0,0,0));
template<typename pointRGBT>
void denseMeshPostProcessing(
pcl::PolygonMeshPtr & mesh,
bool hasColors, // Tell if the mesh has colors
float meshDecimationFactor = 0.0f, // value between 0 and 1, 0=disabled
int maximumPolygons = 0, // 0=disabled
const typename pcl::PointCloud<pointRGBT>::Ptr & cloud = pcl::PointCloud<pointRGBT>::Ptr(), // A RGB point cloud used to transfer colors back to mesh (needed for parameters below)
float transferColorRadius = 0.05f, // <0=disabled, 0=nearest color
bool coloredOutput = true, // If output should be colored
bool cleanMesh = true, // Remove polygons not colored (if coloredOutput is disabled, transferColorRadius is still used to clean the mesh)
int minClusterSize = 50, // Remove small polygon clusters after the mesh has been cleaned (0=disabled)
ProgressState * progressState = 0);
} // namespace util3d
} // namespace rtabmap