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

Marker priors (#859)

Browse Source 
* Added MarkerPriors parameter

* Fixed Marker/Priors format to use '|' instead ';'. Fixed landmark priors not used.

* Marker: added priors variance parameters

* g2o: refactored backward compatibility includes

* fixed build with old g2o
This commit is contained in:
matlabbe
2022-04-28 09:18:30 -04:00
committed by GitHub
parent 190071678f
commit b646c5e1db
14 changed files with 731 additions and 283 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+53
View File
@@ -604,6 +604,44 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDMaxOdomCacheSize(), _maxOdomCacheSize);
Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap);
Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), _markerPriorsLinearVariance);
UASSERT(_markerPriorsLinearVariance>0.0f);
Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceAngular(), _markerPriorsAngularVariance);
UASSERT(_markerPriorsAngularVariance>0.0f);
std::string markerPriorsStr;
if(Parameters::parse(parameters, Parameters::kMarkerPriors(), markerPriorsStr))
{
_markerPriors.clear();
std::list<std::string> strList = uSplit(markerPriorsStr, '|');
for(std::list<std::string>::iterator iter=strList.begin(); iter!=strList.end(); ++iter)
{
std::string markerStr = *iter;
while(!markerStr.empty() && !uIsDigit(markerStr[0]))
{
markerStr.erase(markerStr.begin());
}
if(!markerStr.empty())
{
std::string idStr = uSplitNumChar(markerStr).front();
int id = uStr2Int(idStr);
Transform prior = Transform::fromString(markerStr.substr(idStr.size()));
if(!prior.isNull() && id>0)
{
_markerPriors.insert(std::make_pair(-id, prior));
UDEBUG("Added landmark prior %d: %s", id, prior.prettyPrint().c_str());
}
else
{
UERROR("Failed to parse element \"%s\" in parameter %s", markerStr.c_str(), Parameters::kMarkerPriors().c_str());
}
}
else if(!iter->empty())
{
UERROR("Failed to parse parameter %s, value=\"%s\"", Parameters::kMarkerPriors().c_str(), iter->c_str());
}
}
}
UASSERT(_rgbdLinearUpdate >= 0.0f);
UASSERT(_rgbdAngularUpdate >= 0.0f);
UASSERT(_rgbdLinearSpeedUpdate >= 0.0f);
@@ -1554,6 +1592,7 @@ bool Rtabmap::process(
if(_optimizedPoses.find(iter->first) == _optimizedPoses.end())
{
_optimizedPoses.insert(std::make_pair(iter->first, newPose*iter->second.transform()));
UDEBUG("Added landmark %d : %s", iter->first, (newPose*iter->second.transform()).prettyPrint().c_str());
addedNewLandmark = true;
}
_constraints.insert(std::make_pair(iter->first, iter->second.inverse()));
@@ -4738,6 +4777,20 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase, !_graphOptimizer->landmarksIgnored());
UINFO("get constraints (ids=%d, %d poses, %d edges) time %f s", (int)ids.size(), (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
// add landmark priors if there are some
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && iter->first < 0; ++iter)
{
if(_markerPriors.find(iter->first) != _markerPriors.end())
{
cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1);
infMatrix(cv::Range(0,3), cv::Range(0,3)) /= _markerPriorsLinearVariance;
infMatrix(cv::Range(3,6), cv::Range(3,6)) /= _markerPriorsAngularVariance;
edgeConstraints.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPosePrior, _markerPriors.at(iter->first), infMatrix)));
UDEBUG("Added prior %d : %s (variance: lin=%f ang=%f)", iter->first, _markerPriors.at(iter->first).prettyPrint().c_str(),
_markerPriorsLinearVariance, _markerPriorsAngularVariance);
}
}
if(_graphOptimizer->iterations() > 0)
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)