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 #57: Added 720p option, Export PLY or OBJ, Added Rendering and Mapping menus, RtabmapThread: Fixed large covariance (9999) detection

Browse Source
This commit is contained in:
matlabbe
2016-04-02 15:17:40 -04:00
parent d489cd48e9
commit 30e52b785a
15 changed files with 339 additions and 333 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
corelib/src/RtabmapThread.cpp
View File

@@ -544,14 +544,30 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
ignoreFrame = true;
}
}
if(_dataBufferMaxSize > 0 && !lastPose_.isIdentity() && (odomEvent.pose().isIdentity() || odomEvent.info().variance>=9999))
if(_dataBufferMaxSize > 0 &&
((!lastPose_.isIdentity() && odomEvent.pose().isIdentity()) ||
odomEvent.info().variance>=9999 ||
odomEvent.rotVariance()>=9999 ||
odomEvent.transVariance()>=9999))
{
UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomEvent.info().variance);
UWARN("Odometry is reset (identity pose or high variance (>=9999) detected). Increment map id!");
pushNewState(kStateTriggeringMap);
_rotVariance = 0;
_transVariance = 0;
}
double maxRotVar = odomEvent.rotVariance();
double maxTransVar = odomEvent.transVariance();
// FIXME: should merge the transformations/variances like Link::merge();
if(maxRotVar > _rotVariance)
{
_rotVariance = maxRotVar;
}
if(maxTransVar > _transVariance)
{
_transVariance = maxTransVar;
}
if(ignoreFrame && !_createIntermediateNodes)
{
return;
@@ -563,17 +579,6 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
}
lastPose_ = odomEvent.pose();
double maxRotVar = odomEvent.rotVariance();
double maxTransVar = odomEvent.transVariance();
// FIXME: should merge the transformations/variances like Link::merge();
if(maxRotVar > _rotVariance)
{
_rotVariance = maxRotVar;
}
if(maxTransVar > _transVariance)
{
_transVariance = maxTransVar;
}
bool notify = true;
_dataMutex.lock();
@@ -598,7 +603,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
{
_dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), _rotVariance, _transVariance));
}
UDEBUG("Added data %d", odomEvent.data().id());
UINFO("Added data %d (variance=%f)", odomEvent.data().id(), _rotVariance);
_rotVariance = 0;
_transVariance = 0;

64
corelib/src/util3d_surface.cpp
View File

@@ -194,6 +194,32 @@ void appendMesh(
}
}
void appendMesh(
pcl::PointCloud<pcl::PointXYZRGB> & cloudA,
std::vector<pcl::Vertices> & polygonsA,
const pcl::PointCloud<pcl::PointXYZRGB> & cloudB,
const std::vector<pcl::Vertices> & polygonsB)
{
UDEBUG("cloudA=%d polygonsA=%d cloudB=%d polygonsB=%d", (int)cloudA.size(), (int)polygonsA.size(), (int)cloudB.size(), (int)polygonsB.size());
UASSERT(!cloudA.isOrganized() && !cloudB.isOrganized());
int sizeA = cloudA.size();
cloudA += cloudB;
int sizePolygonsA = polygonsA.size();
polygonsA.resize(sizePolygonsA+polygonsB.size());
for(unsigned int i=0; i<polygonsB.size(); ++i)
{
pcl::Vertices vertices = polygonsB[i];
for(unsigned int j=0; j<vertices.vertices.size(); ++j)
{
vertices.vertices[j] += sizeA;
}
polygonsA[i+sizePolygonsA] = vertices;
}
}
std::map<int, int> filterNotUsedVerticesFromMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
const std::vector<pcl::Vertices> & polygons,
@@ -232,6 +258,44 @@ std::map<int, int> filterNotUsedVerticesFromMesh(
return output;
}
std::map<int, int> filterNotUsedVerticesFromMesh(
const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
const std::vector<pcl::Vertices> & polygons,
pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
std::vector<pcl::Vertices> & outputPolygons)
{
UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
std::map<int, int> addedVertices; //<oldIndex, newIndex>
std::map<int, int> output; //<newIndex, oldIndex>
outputCloud.resize(cloud.size());
outputCloud.is_dense = true;
outputPolygons.resize(polygons.size());
int oi = 0;
for(unsigned int i=0; i<polygons.size(); ++i)
{
pcl::Vertices & v = outputPolygons[i];
v.vertices.resize(polygons[i].vertices.size());
for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
{
std::map<int, int>::iterator iter = addedVertices.find(polygons[i].vertices[j]);
if(iter == addedVertices.end())
{
outputCloud[oi] = cloud.at(polygons[i].vertices[j]);
addedVertices.insert(std::make_pair(polygons[i].vertices[j], oi));
output.insert(std::make_pair(oi, polygons[i].vertices[j]));
v.vertices[j] = oi++;
}
else
{
v.vertices[j] = iter->second;
}
}
}
outputCloud.resize(oi);
return output;
}
std::vector<pcl::Vertices> filterCloseVerticesFromMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud,
const std::vector<pcl::Vertices> & polygons,