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

Added RGBD/ProximityMergedScanCovFactor parameter. Localization: fixed output height when Reg/Force3DoF=true but input poses are 6DoF. Transform: added is3DoF() and is4DoF() functions. GTSAM: when Reg/Force3DoF=true, copy input roll,pitch,z values for output poses. RegIcp: fixed working memory dir '~' conversion. RGBD/ProximityGlobalScanMap: fixed map::at error when some nodes don't have scans.

Browse Source
This commit is contained in:
matlabbe
2022-01-12 16:14:42 -05:00
parent a4ec95963e
commit fcec98105d
14 changed files with 122 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/Graph.cpp
View File

@@ -1336,7 +1336,7 @@ std::map<int, Transform> radiusPosesFiltering(
//pcl::IndicesPtr indicesOut(new std::vector<int>);
//indicesOut->insert(indicesOut->end(), indicesKept.begin(), indicesKept.end());
UINFO("Cloud filtered In = %d, Out = %d", cloud->size(), indicesKept.size());
UINFO("Cloud filtered In = %d, Out = %d (radius=%f angle=%f keepLatest=%d)", cloud->size(), indicesKept.size(), radius, angle, keepLatest?1:0);
//pcl::io::savePCDFile("duplicateIn.pcd", *cloud);
//pcl::io::savePCDFile("duplicateOut.pcd", *cloud, *indicesOut);

26
corelib/src/Optimizer.cpp
View File

@@ -190,7 +190,8 @@ void Optimizer::getConnectedGraph(
const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & linksIn,
std::map<int, Transform> & posesOut,
std::multimap<int, Link> & linksOut) const
std::multimap<int, Link> & linksOut,
bool adjustPosesWithConstraints) const
{
UDEBUG("IN: fromId=%d poses=%d links=%d priorsIgnored=%d landmarksIgnored=%d", fromId, (int)posesIn.size(), (int)linksIn.size(), priorsIgnored()?1:0, landmarksIgnored()?1:0);
UASSERT(fromId>0);
@@ -243,23 +244,30 @@ void Optimizer::getConnectedGraph(
{
if(!uContains(posesOut, toId))
{
if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0)
if(adjustPosesWithConstraints)
{
Transform t;
if(kter->second.from()==currentId)
if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0)
{
t = kter->second.transform();
Transform t;
if(kter->second.from()==currentId)
{
t = kter->second.transform();
}
else
{
t = kter->second.transform().inverse();
}
posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
}
else
{
t = kter->second.transform().inverse();
Transform t = posesOut.at(currentId) * (kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse());
posesOut.insert(std::make_pair(toId, t));
}
posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
}
else
{
Transform t = posesOut.at(currentId) * (kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse());
posesOut.insert(std::make_pair(toId, t));
posesOut.insert(*posesIn.find(toId));
}
// add prior links
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)

2
corelib/src/RegistrationIcp.cpp
View File

@@ -136,7 +136,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
{
_workingDir = iter->second;
_workingDir = uReplaceChar(iter->second, '~', UDirectory::homeDir());
}
bool pointToPlane = _pointToPlane;

44
corelib/src/Rtabmap.cpp
View File

@@ -121,6 +121,7 @@ Rtabmap::Rtabmap() :
_proximityRawPosesUsed(Parameters::defaultRGBDProximityPathRawPosesUsed()),
_proximityAngle(Parameters::defaultRGBDProximityAngle()*M_PI/180.0f),
_proximityOdomGuess(Parameters::defaultRGBDProximityOdomGuess()),
_proximityMergedScanCovFactor(Parameters::defaultRGBDProximityMergedScanCovFactor()),
_databasePath(""),
_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
_optimizationMaxError(Parameters::defaultRGBDOptimizeMaxError()),
@@ -573,6 +574,9 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
_proximityAngle *= M_PI/180.0f;
}
Parameters::parse(parameters, Parameters::kRGBDProximityOdomGuess(), _proximityOdomGuess);
Parameters::parse(parameters, Parameters::kRGBDProximityMergedScanCovFactor(), _proximityMergedScanCovFactor);
UASSERT(_proximityMergedScanCovFactor>0.0);
bool optimizeFromGraphEndPrevious = _optimizeFromGraphEnd;
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
if(optimizeFromGraphEndPrevious != _optimizeFromGraphEnd && !_optimizedPoses.empty())
@@ -2586,7 +2590,7 @@ bool Rtabmap::process(
// Assemble scans in the path and do ICP only
std::map<int, Transform> optimizedLocalPath;
if(_proximityRawPosesUsed)
if(_globalScanMap.empty() && _proximityRawPosesUsed)
{
//optimize the path's poses locally
cv::Mat covariance;
@@ -2610,7 +2614,7 @@ bool Rtabmap::process(
}
std::map<int, Transform> filteredPath;
if(optimizedLocalPath.size() > 2 && proximityFilteringRadius > 0.0f)
if(_globalScanMap.empty() && optimizedLocalPath.size() > 2 && proximityFilteringRadius > 0.0f)
{
// path filtering
filteredPath = graph::radiusPosesFiltering(optimizedLocalPath, proximityFilteringRadius, 0, true);
@@ -2639,6 +2643,7 @@ bool Rtabmap::process(
}
else
{
UASSERT_MSG(_globalScanMapPoses.find(nearestId) != _globalScanMapPoses.end(), uFormat("Pose of %d not found in global scan poses", nearestId).c_str());
icpMulti = false;
// use pre-assembled scan map
SensorData assembledData;
@@ -2684,7 +2689,7 @@ bool Rtabmap::process(
// set Identify covariance for laser scan matching only
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, getInformation(info.covariance)/100.0, scanMatchingIds));
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, getInformation(info.covariance)/_proximityMergedScanCovFactor, scanMatchingIds));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
if(icpMulti)
@@ -2925,7 +2930,8 @@ bool Rtabmap::process(
bool priorsIgnored = _graphOptimizer->priorsIgnored();
UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false");
_graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut);
// If slam2d: get connected graph while keeping original roll,pitch,z values.
_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut, !_graphOptimizer->isSlam2d());
std::map<int, Transform> optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut);
_graphOptimizer->setPriorsIgnored(priorsIgnored); // set back
for(std::map<int, Transform>::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter)
@@ -3067,12 +3073,7 @@ bool Rtabmap::process(
Transform oldPose = _optimizedPoses.at(localizationLinks.rbegin()->first);
Transform mapCorrectionInv = _mapCorrection.inverse();
Transform u = signature->getPose() * localizationLinks.rbegin()->second.transform();
if(_graphOptimizer->isSlam2d())
{
// in case of 3d landmarks, transform constraint to 2D
u = u.to3DoF();
}
else if(_graphOptimizer->gravitySigma() > 0)
if(_graphOptimizer->gravitySigma() > 0)
{
// Adjust transform with gravity
Transform transform = localizationLinks.rbegin()->second.transform();
@@ -3122,6 +3123,11 @@ bool Rtabmap::process(
}
}
Transform up = u * oldPose.inverse();
if(_graphOptimizer->isSlam2d())
{
// in case of 3d landmarks, transform constraint to 2D
up.to3DoF();
}
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{
iter->second = mapCorrectionInv * up * iter->second;
@@ -3132,7 +3138,7 @@ bool Rtabmap::process(
{
Transform newPose = _optimizedPoses.at(localizationLinks.rbegin()->first) * localizationLinks.rbegin()->second.transform().inverse();
UDEBUG("newPose=%s", newPose.prettyPrint().c_str());
if(_graphOptimizer->isSlam2d())
if(_graphOptimizer->isSlam2d() && signature->getPose().is3DoF())
{
// in case of 3d landmarks, transform constraint to 2D
newPose = newPose.to3DoF();
@@ -3607,10 +3613,8 @@ bool Rtabmap::process(
Signature lastSignatureData(signature->id());
Transform lastSignatureLocalizedPose;
if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end() &&
graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size())
if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end())
{
// only if localized set it
lastSignatureLocalizedPose = _optimizedPoses.at(signature->id());
}
if(_publishLastSignatureData)
@@ -3762,8 +3766,8 @@ bool Rtabmap::process(
if(signaturesRemoved.size() == 1 && signaturesRemoved.front() == lastSignatureData.id())
{
UDEBUG("Detected that only last signature has been removed");
int lastId = signaturesRemoved.front();
UDEBUG("Detected that only last signature has been removed (lastId=%d)", lastId);
_optimizedPoses.erase(lastId);
for(std::multimap<int, Link>::iterator iter=_constraints.find(lastId); iter!=_constraints.end() && iter->first==lastId;++iter)
{
@@ -6319,6 +6323,16 @@ void Rtabmap::createGlobalScanMap()
break;
}
}
else
{
UDEBUG("Ignored %d (scan is empty), pose still added.", iter->first);
_globalScanMapPoses.insert(*iter);
}
}
else
{
UDEBUG("Ignored %d (no scan), pose still added.", iter->first);
_globalScanMapPoses.insert(*iter);
}
}
if(_globalScanMap.size() > 3)

14
corelib/src/Transform.cpp
View File

@@ -221,6 +221,20 @@ Transform Transform::to4DoF() const
return Transform(x,y,z, 0,0,yaw);
}
bool Transform::is3DoF() const
{
return is4DoF() && z() == 0.0;
}
bool Transform::is4DoF() const
{
return r13() == 0.0 &&
r23() == 0.0 &&
r31() == 0.0 &&
r32() == 0.0 &&
r33() == 0.0;
}
cv::Mat Transform::rotationMatrix() const
{
return data_.colRange(0, 3).clone();

2
corelib/src/optimizer/OptimizerG2O.cpp
View File

@@ -359,7 +359,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
int landmarkVertexOffset = poses.rbegin()->first+1;
std::map<int, bool> isLandmarkWithRotation;
UDEBUG("fill poses to g2o... (rootId=%d hasGravityConstraints=%d)", rootId, hasGravityConstraints?1:0);
UDEBUG("fill poses to g2o... (rootId=%d hasGravityConstraints=%d isSlam2d=%d)", rootId, hasGravityConstraints?1:0, isSlam2d()?1:0);
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());

3
corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -625,8 +625,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
if(key > 0)
{
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta())));
optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta())));
}
else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
{