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

GUI: Added "Edit->Post processing..." action to detect more loop closures and/or refine all links in the graph with ICP 3D.

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1940 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-30 14:43:43 +00:00
parent 4fbefc9ec5
commit 1597cfc293
20 changed files with 1328 additions and 446 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/Memory.h
View File

@@ -245,6 +245,8 @@ private:
float _icpMaxCorrespondenceDistance;
int _icpMaxIterations;
float _icpMaxFitness;
bool _icpPointToPlane;
int _icpPointToPlaneNormalNeighbors;
float _icp2MaxCorrespondenceDistance;
int _icp2MaxIterations;
float _icp2MaxFitness;

4
corelib/include/rtabmap/core/Parameters.h
View File

@@ -302,11 +302,13 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(LccIcp3, Decimation, int, 8, "Depth image decimation.");
RTABMAP_PARAM(LccIcp3, MaxDepth, float, 4.0, "Max cloud depth.");
RTABMAP_PARAM(LccIcp3, VoxelSize, float, 0.005, "Voxel size to be used for ICP computation.");
RTABMAP_PARAM(LccIcp3, VoxelSize, float, 0.01, "Voxel size to be used for ICP computation.");
RTABMAP_PARAM(LccIcp3, Samples, int, 0, "Random samples to be used for ICP computation. Not used if voxelSize is set.");
RTABMAP_PARAM(LccIcp3, MaxCorrespondenceDistance, float, 0.05, "ICP 3D: Max distance for point correspondences.");
RTABMAP_PARAM(LccIcp3, Iterations, int, 30, "ICP 3D: Max iterations.");
RTABMAP_PARAM(LccIcp3, MaxFitness, float, 1.0, "ICP 3D: Maximum fitness to accept the computed transform.");
RTABMAP_PARAM(LccIcp3, PointToPlane, bool, false, "ICP 3D: Use point to plane ICP.");
RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors, int, 20, "ICP 3D: Number of neighbors to compute normals for point to plane.");
RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.1, "ICP 2D: Max distance for point correspondences.");
RTABMAP_PARAM(LccIcp2, Iterations, int, 30, "ICP 2D: Max iterations.");

21
corelib/include/rtabmap/core/util3d.h
View File

@@ -443,11 +443,30 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(
float gp3MaximumAngle = 2*M_PI/3,
bool gp3NormalConsistency = false);
std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
std::multimap<int, Link> & links,
int from,
int to);
// <int, depth> depth=0 means infinite depth
std::map<int, int> RTABMAP_EXP generateDepthGraph(
const std::multimap<int, Link> & links,
int fromId,
int depth = 0);
void RTABMAP_EXP optimizeTOROGraph(
const std::map<int, int> & depthGraph,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
std::map<int, Transform> & optimizedPoses,
int toroIterations = 100,
bool toroInitialGuess = true,
std::list<std::map<int, Transform> > * intermediateGraphes = 0);
void RTABMAP_EXP optimizeTOROGraph(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::map<int, Transform> & optimizedPoses,
Transform & mapCorrection,
int toroIterations = 100,
bool toroInitialGuess = true,
std::list<std::map<int, Transform> > * intermediateGraphes = 0);

71
corelib/src/Memory.cpp
View File

@@ -95,6 +95,8 @@ Memory::Memory(const ParametersMap & parameters) :
_icpMaxCorrespondenceDistance(Parameters::defaultLccIcp3MaxCorrespondenceDistance()),
_icpMaxIterations(Parameters::defaultLccIcp3Iterations()),
_icpMaxFitness(Parameters::defaultLccIcp3MaxFitness()),
_icpPointToPlane(Parameters::defaultLccIcp3PointToPlane()),
_icpPointToPlaneNormalNeighbors(Parameters::defaultLccIcp3PointToPlaneNormalNeighbors()),
_icp2MaxCorrespondenceDistance(Parameters::defaultLccIcp2MaxCorrespondenceDistance()),
_icp2MaxIterations(Parameters::defaultLccIcp2Iterations()),
@@ -403,6 +405,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), _icpMaxCorrespondenceDistance);
Parameters::parse(parameters, Parameters::kLccIcp3Iterations(), _icpMaxIterations);
Parameters::parse(parameters, Parameters::kLccIcp3MaxFitness(), _icpMaxFitness);
Parameters::parse(parameters, Parameters::kLccIcp3PointToPlane(), _icpPointToPlane);
Parameters::parse(parameters, Parameters::kLccIcp3PointToPlaneNormalNeighbors(), _icpPointToPlaneNormalNeighbors);
Parameters::parse(parameters, Parameters::kLccIcp2MaxCorrespondenceDistance(), _icp2MaxCorrespondenceDistance);
Parameters::parse(parameters, Parameters::kLccIcp2Iterations(), _icp2MaxIterations);
Parameters::parse(parameters, Parameters::kLccIcp2MaxFitness(), _icp2MaxFitness);
@@ -429,6 +433,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
UASSERT_MSG(_icpMaxCorrespondenceDistance > 0.0f, uFormat("value=%f", _icpMaxCorrespondenceDistance).c_str());
UASSERT_MSG(_icpMaxIterations > 0, uFormat("value=%d", _icpMaxIterations).c_str());
UASSERT_MSG(_icpMaxFitness > 0.0f, uFormat("value=%f", _icpMaxFitness).c_str());
UASSERT_MSG(_icpPointToPlaneNormalNeighbors > 0, uFormat("value=%d", _icpPointToPlaneNormalNeighbors).c_str());
UASSERT_MSG(_icp2MaxCorrespondenceDistance > 0.0f, uFormat("value=%f", _icp2MaxCorrespondenceDistance).c_str());
UASSERT_MSG(_icp2MaxIterations > 0, uFormat("value=%d", _icp2MaxIterations).c_str());
UASSERT_MSG(_icp2MaxFitness > 0.0f, uFormat("value=%f", _icp2MaxFitness).c_str());
@@ -1844,28 +1849,17 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, boo
//make sure data are uncompressed
if(icp3D)
{
if(oldS->getDepthRaw().empty())
{
oldS->setDepthRaw(util3d::uncompressImage(oldS->getDepthCompressed()));
}
if(newS->getDepthRaw().empty())
{
newS->setDepthRaw(util3d::uncompressImage(newS->getDepthCompressed()));
}
cv::Mat tmp1, tmp2;
oldS->uncompressData(0, &tmp1, 0);
newS->uncompressData(0, &tmp2, 0);
}
else
{
if(oldS->getDepth2DRaw().empty())
{
oldS->setDepth2DRaw(util3d::uncompressData(oldS->getDepth2DCompressed()));
}
if(newS->getDepth2DRaw().empty())
{
newS->setDepth2DRaw(util3d::uncompressData(newS->getDepth2DCompressed()));
}
cv::Mat tmp1, tmp2;
oldS->uncompressData(0, 0, &tmp1);
newS->uncompressData(0, 0, &tmp2);
}
t = computeIcpTransform(*oldS, *newS, guess, icp3D, rejectedMsg);
}
else
@@ -1933,25 +1927,40 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
_icpSamples,
guess * newS.getLocalTransform());
pcl::PointCloud<pcl::PointNormal>::Ptr oldCloud = util3d::computeNormals(oldCloudXYZ);
pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
std::vector<int> indices;
newCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(newCloud);
oldCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(oldCloud);
// 3D
double fitness = 0;
bool hasConverged = false;
Transform icpT;
if(newCloud->size() && oldCloud->size())
if(newCloudXYZ->size() && oldCloudXYZ->size())
{
icpT = util3d::icpPointToPlane(newCloud,
oldCloud,
_icpMaxCorrespondenceDistance,
_icpMaxIterations,
hasConverged,
fitness);
if(_icpPointToPlane)
{
pcl::PointCloud<pcl::PointNormal>::Ptr oldCloud = util3d::computeNormals(oldCloudXYZ, _icpPointToPlaneNormalNeighbors);
pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ, _icpPointToPlaneNormalNeighbors);
std::vector<int> indices;
newCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(newCloud);
oldCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(oldCloud);
if(newCloud->size() && oldCloud->size())
{
icpT = util3d::icpPointToPlane(newCloud,
oldCloud,
_icpMaxCorrespondenceDistance,
_icpMaxIterations,
hasConverged,
fitness);
}
}
else
{
transform = util3d::icp(newCloudXYZ,
oldCloudXYZ,
_icpMaxCorrespondenceDistance,
_icpMaxIterations,
hasConverged,
fitness);
}
//pcl::io::savePCDFile("old.pcd", *oldCloudXYZ);
//pcl::io::savePCDFile("newguess.pcd", *newCloudXYZ);

73
corelib/src/Rtabmap.cpp
View File

@@ -2002,78 +2002,7 @@ void Rtabmap::optimizeCurrentMap(
}
else
{
// Modify IDs using the margin from the current signature (TORO root will be the last signature)
int m = 0;
int toroId = 1;
std::map<int, int> rtabmapToToro; // <RTAB-Map ID, TORO ID>
std::map<int, int> toroToRtabmap; // <TORO ID, RTAB-Map ID>
while(ids.size())
{
for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end();)
{
if(m == iter->second)
{
rtabmapToToro.insert(std::make_pair(iter->first, toroId));
toroToRtabmap.insert(std::make_pair(toroId, iter->first));
++toroId;
ids.erase(iter++);
}
else
{
++iter;
}
}
++m;
}
//
std::map<int, Transform> posesToro;
std::multimap<int, Link> edgeConstraintsToro;
for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
posesToro.insert(std::make_pair(rtabmapToToro.at(iter->first), iter->second));
}
for(std::multimap<int, Link>::iterator iter = edgeConstraints.begin();
iter!=edgeConstraints.end();
++iter)
{
edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.transform(), iter->second.type())));
}
if(posesToro.size() > 1 && edgeConstraintsToro.size() > 0)
{
///UDEBUG("TORO optimize begin");
//util3d::saveTOROGraph("toroIdRtabmap.graph", poses, edgeConstraints);
//util3d::saveTOROGraph("toroIdToro.graph", posesToro, edgeConstraintsToro);
//UDEBUG("graph saved");
std::map<int, Transform> optimizedPosesToro;
Transform mapCorrectionToro;
util3d::optimizeTOROGraph(posesToro, edgeConstraintsToro, optimizedPosesToro, mapCorrectionToro, _toroIterations, true);
//UDEBUG("saving optimized graph...");
//util3d::saveTOROGraph("toroOptimized.graph", optimizedPosesToro, edgeConstraintsToro);
//UDEBUG("TORO optimize end");
for(std::map<int, Transform>::iterator iter=optimizedPosesToro.begin(); iter!=optimizedPosesToro.end(); ++iter)
{
optimizedPoses.insert(std::make_pair(toroToRtabmap.at(iter->first), iter->second));
}
//util3d::saveTOROGraph("toro.graph", poses, edgeConstraints);
//util3d::saveTOROGraph("toroOptimized.graph", _optimizedPoses, edgeConstraints);
}
else if(poses.size() == 1 && edgeConstraints.size() == 0)
{
optimizedPoses = poses;
}
else if(poses.size() || edgeConstraints.size())
{
UFATAL("Poses=%d and edges=%d (poses must "
"not be null if there are edges, and edges must be null if poses <= 1)",
poses.size(), edgeConstraints.size());
}
util3d::optimizeTOROGraph(ids, poses, edgeConstraints, optimizedPoses, _toroIterations, true);
}
}
}

206
corelib/src/util3d.cpp
View File

@@ -2049,16 +2049,201 @@ pcl::PolygonMesh::Ptr createMesh(
return mesh;
}
std::multimap<int, Link>::iterator findLink(
std::multimap<int, Link> & links,
int from,
int to)
{
std::multimap<int, Link>::iterator iter = links.find(from);
while(iter != links.end() && iter->first == from)
{
if(iter->second.to() == to)
{
return iter;
}
++iter;
}
// let's try to -> from
iter = links.find(to);
while(iter != links.end() && iter->first == to)
{
if(iter->second.to() == from)
{
return iter;
}
++iter;
}
return links.end();
}
// <int, depth> margin=0 means infinite margin
std::map<int, int> generateDepthGraph(
const std::multimap<int, Link> & links,
int fromId,
int depth)
{
UASSERT(depth >= 0);
//UDEBUG("signatureId=%d, neighborsMargin=%d", signatureId, margin);
std::map<int, int> ids;
if(fromId<=0)
{
return ids;
}
std::list<int> curentDepthList;
std::set<int> nextDepth;
nextDepth.insert(fromId);
int d = 0;
while((depth == 0 || d < depth) && nextDepth.size())
{
curentDepthList = std::list<int>(nextDepth.begin(), nextDepth.end());
nextDepth.clear();
for(std::list<int>::iterator jter = curentDepthList.begin(); jter!=curentDepthList.end(); ++jter)
{
if(ids.find(*jter) == ids.end())
{
std::set<int> marginIds;
ids.insert(std::pair<int, int>(*jter, d));
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.from() == *jter)
{
marginIds.insert(iter->second.to());
}
else if(iter->second.to() == *jter)
{
marginIds.insert(iter->second.from());
}
}
// Margin links
for(std::set<int>::const_iterator iter=marginIds.begin(); iter!=marginIds.end(); ++iter)
{
if( !uContains(ids, *iter) && nextDepth.find(*iter) == nextDepth.end())
{
nextDepth.insert(*iter);
}
}
}
}
++d;
}
return ids;
}
void optimizeTOROGraph(
const std::map<int, int> & depthGraph,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
std::map<int, Transform> & optimizedPoses,
int toroIterations,
bool toroInitialGuess,
std::list<std::map<int, Transform> > * intermediateGraphes)
{
optimizedPoses.clear();
if(depthGraph.size() && poses.size()>=2 && links.size()>=1)
{
// Modify IDs using the margin from the current signature (TORO root will be the last signature)
int m = 0;
int toroId = 1;
std::map<int, int> rtabmapToToro; // <RTAB-Map ID, TORO ID>
std::map<int, int> toroToRtabmap; // <TORO ID, RTAB-Map ID>
std::map<int, int> idsTmp = depthGraph;
while(idsTmp.size())
{
for(std::map<int, int>::iterator iter = idsTmp.begin(); iter!=idsTmp.end();)
{
if(m == iter->second)
{
rtabmapToToro.insert(std::make_pair(iter->first, toroId));
toroToRtabmap.insert(std::make_pair(toroId, iter->first));
++toroId;
idsTmp.erase(iter++);
}
else
{
++iter;
}
}
++m;
}
std::map<int, rtabmap::Transform> posesToro;
std::multimap<int, rtabmap::Link> edgeConstraintsToro;
for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(uContains(depthGraph, iter->first))
{
posesToro.insert(std::make_pair(rtabmapToToro.at(iter->first), iter->second));
}
}
for(std::multimap<int, rtabmap::Link>::const_iterator iter = links.begin();
iter!=links.end();
++iter)
{
if(uContains(depthGraph, iter->second.from()) && uContains(depthGraph, iter->second.to()))
{
edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), rtabmap::Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.transform(), iter->second.type())));
}
}
std::map<int, rtabmap::Transform> optimizedPosesToro;
// Optimize!
if(posesToro.size() && edgeConstraintsToro.size())
{
std::list<std::map<int, rtabmap::Transform> > graphesToro;
rtabmap::util3d::optimizeTOROGraph(posesToro, edgeConstraintsToro, optimizedPosesToro, toroIterations, toroInitialGuess, &graphesToro);
for(std::map<int, rtabmap::Transform>::iterator iter=optimizedPosesToro.begin(); iter!=optimizedPosesToro.end(); ++iter)
{
optimizedPoses.insert(std::make_pair(toroToRtabmap.at(iter->first), iter->second));
}
if(intermediateGraphes)
{
for(std::list<std::map<int, rtabmap::Transform> >::iterator iter = graphesToro.begin(); iter!=graphesToro.end(); ++iter)
{
std::map<int, rtabmap::Transform> tmp;
for(std::map<int, rtabmap::Transform>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
{
tmp.insert(std::make_pair(toroToRtabmap.at(jter->first), jter->second));
}
intermediateGraphes->push_back(tmp);
}
}
}
else
{
UERROR("No TORO poses and constraints!?");
}
}
else if(links.size() == 0 && poses.size() == 1)
{
optimizedPoses = poses;
}
else
{
UERROR("Wrong inputs! depthGraph=%d poses=%d links=%d",
(int)depthGraph.size(), (int)poses.size(), (int)links.size());
}
}
//On success, optimizedPoses is cleared and new poses are inserted in
void optimizeTOROGraph(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::map<int, Transform> & optimizedPoses,
Transform & mapCorrection,
int toroIterations,
bool toroInitialGuess,
std::list<std::map<int, Transform> > * intermediateGraphes) // contains poses after tree init to last one before the end
{
UASSERT(toroIterations>0);
optimizedPoses.clear();
if(edgeConstraints.size()>=1 && poses.size()>=2)
{
// Apply TORO optimization
@@ -2133,7 +2318,6 @@ void optimizeTOROGraph(
}
UDEBUG("TORO iterate end");
optimizedPoses.clear();
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v=pg.vertex(iter->first);
@@ -2143,15 +2327,19 @@ void optimizeTOROGraph(
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
Eigen::Matrix4f newPose = transformToEigen4f(optimizedPoses.at(poses.rbegin()->first));
Eigen::Matrix4f oldPose = transformToEigen4f(poses.rbegin()->second);
Eigen::Matrix4f poseCorrection = oldPose.inverse() * newPose; // transform from odom to correct odom
Eigen::Matrix4f result = oldPose*poseCorrection*oldPose.inverse();
mapCorrection = transformFromEigen4f(result);
//Eigen::Matrix4f newPose = transformToEigen4f(optimizedPoses.at(poses.rbegin()->first));
//Eigen::Matrix4f oldPose = transformToEigen4f(poses.rbegin()->second);
//Eigen::Matrix4f poseCorrection = oldPose.inverse() * newPose; // transform from odom to correct odom
//Eigen::Matrix4f result = oldPose*poseCorrection*oldPose.inverse();
//mapCorrection = transformFromEigen4f(result);
}
else if(edgeConstraints.size() == 0 && poses.size() == 1)
{
optimizedPoses = poses;
}
else
{
UWARN("This method should be called at least with 2 poses and one link!");
UWARN("This method should be called at least with 1 pose!");
}
}
@@ -2707,7 +2895,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
pcl::PointCloud<pcl::PointXYZ> minMax;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(uContains(scans, iter->first))
if(uContains(scans, iter->first) && scans.size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = transformPointCloud<pcl::PointXYZ>(scans.at(iter->first), iter->second);
pcl::PointXYZ min, max;