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

Tango: Added Settings/Optimized Mesh/Sketchfab Upload

Browse Source
This commit is contained in:
matlabbe
2017-01-26 16:46:24 -05:00
parent 66ee792e8c
commit 3a73414972
27 changed files with 4441 additions and 2029 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
corelib/src/Rtabmap.cpp
View File

@@ -1344,13 +1344,26 @@ bool Rtabmap::process(
if(_highestHypothesis.first > 0)
{
float loopThr = _loopThr;
if((_startNewMapOnLoopClosure || !_memory->isIncremental()) &&
signature->getLinks().size() == 0 && // alone in the current map
_memory->getWorkingMem().size()>1 && // should have an old map (beside virtual signature)
(int)_memory->getWorkingMem().size()<=_memory->getMaxStMemSize() &&
_rgbdSlamMode)
{
// If the map is very small (under STM size) and we need to find
// a loop closure before continuing the map or localizing,
// use the best hypothesis directly.
loopThr = 0.0f;
}
// Loop closure Threshold
// When _loopThr=0, accept loop closure if the hypothesis is over
// the virtual (new) place hypothesis.
if(_highestHypothesis.second >= _loopThr)
if(_highestHypothesis.second >= loopThr)
{
rejectedHypothesis = true;
if(posterior.size() <= 2)
if(posterior.size() <= 2 && loopThr>0.0f)
{
// Ignore loop closure if there is only one loop closure hypothesis
UDEBUG("rejected hypothesis: single hypothesis");
@@ -1380,7 +1393,7 @@ bool Rtabmap::process(
else if(_highestHypothesis.second < _loopRatio*lastHighestHypothesis.second)
{
// Used for Precision-Recall computation.
// When analysing logs, it's convenient to know
// When analyzing logs, it's convenient to know
// if the hypothesis would be rejected if T_loop would be lower.
rejectedHypothesis = true;
UWARN("rejected hypothesis: under loop ratio %f < %f", _highestHypothesis.second, _loopRatio*lastHighestHypothesis.second);
@@ -2410,7 +2423,7 @@ bool Rtabmap::process(
if(_startNewMapOnLoopClosure &&
_memory->isIncremental() && // only in mapping mode
signature->getLinks().size() == 0 && // alone in the current map
_memory->getWorkingMem().size()>1) // The working memory should not be empty
_memory->getWorkingMem().size()>=2) // The working memory should not be empty (beside virtual signature)
{
UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
signature->id());

88
corelib/src/util3d_surface.cpp
View File

@@ -609,6 +609,7 @@ pcl::TextureMesh::Ptr createTextureMesh(
const std::map<int, CameraModel> & cameraModels,
int kNormalSearch)
{
UASSERT(mesh->polygons.size());
pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
textureMesh->cloud = mesh->cloud;
textureMesh->tex_polygons.push_back(mesh->polygons);
@@ -666,24 +667,81 @@ pcl::TextureMesh::Ptr createTextureMesh(
// compute normals for the mesh if not already here
bool hasNormals = false;
bool hasColors = false;
for(unsigned int i=0; i<textureMesh->cloud.fields.size(); ++i)
{
if(textureMesh->cloud.fields[i].name.compare("normal_x") == 0)
{
hasNormals = true;
break;
}
else if(textureMesh->cloud.fields[i].name.compare("rgb") == 0)
{
hasColors = true;
}
}
if(!hasNormals)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
pcl::PointCloud<pcl::Normal>::Ptr normals = computeNormals(cloud, kNormalSearch);
// Concatenate the XYZ and normal fields
pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields (*cloud, *normals, *cloudWithNormals);
textureMesh->cloud = pcl::PCLPointCloud2();
pcl::toPCLPointCloud2 (*cloudWithNormals, textureMesh->cloud);
// use polygons
if(hasColors)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
for(unsigned int i=0; i<mesh->polygons.size(); ++i)
{
pcl::Vertices & v = mesh->polygons[i];
UASSERT(v.vertices.size()>2);
Eigen::Vector3f v0(
cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
int last = v.vertices.size()-1;
Eigen::Vector3f v1(
cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
Eigen::Vector3f normal = v0.cross(v1);
normal.normalize();
// flat normal (per face)
for(unsigned int j=0; j<v.vertices.size(); ++j)
{
cloud->at(v.vertices[j]).normal_x = normal[0];
cloud->at(v.vertices[j]).normal_y = normal[1];
cloud->at(v.vertices[j]).normal_z = normal[2];
}
}
pcl::toPCLPointCloud2 (*cloud, textureMesh->cloud);
}
else
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
for(unsigned int i=0; i<mesh->polygons.size(); ++i)
{
pcl::Vertices & v = mesh->polygons[i];
UASSERT(v.vertices.size()>2);
Eigen::Vector3f v0(
cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
int last = v.vertices.size()-1;
Eigen::Vector3f v1(
cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
Eigen::Vector3f normal = v0.cross(v1);
normal.normalize();
// flat normal (per face)
for(unsigned int j=0; j<v.vertices.size(); ++j)
{
cloud->at(v.vertices[j]).normal_x = normal[0];
cloud->at(v.vertices[j]).normal_y = normal[1];
cloud->at(v.vertices[j]).normal_z = normal[2];
}
}
pcl::toPCLPointCloud2 (*cloud, textureMesh->cloud);
}
}
return textureMesh;
@@ -797,18 +855,30 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
{
UASSERT(cloud->isOrganized());
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
if(indices->size())
{
tree->setInputCloud(cloud, indices);
}
else
{
tree->setInputCloud (cloud);
}
// Normal estimation
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
ne.setMaxDepthChangeFactor(maxDepthChangeFactor);
ne.setNormalSmoothingSize(normalSmoothingSize);
ne.setBorderPolicy(ne.BORDER_POLICY_MIRROR);
ne.setInputCloud(cloud);
// Commented: Keep the output normals size the same as the input cloud
//if(indices->size())
//{
// ne.setIndices(indices);
//}
ne.setSearchMethod(tree);
ne.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
ne.compute(*normals);