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

updated for rtabmap standalone 0.8.7

Browse Source
This commit is contained in:
Mathieu Labbe
2015-03-12 17:01:29 -04:00
parent 535f8d27c0
commit 6400396509
3 changed files with 23 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>rtabmap_ros</name>
<version>0.8.6</version>
<version>0.8.7</version>
<description>RTAB-Map's ros-pkg. RTAB-Map is an RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
src/CoreWrapper.cpp
+11 -4
View File
@@ -259,6 +259,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
oldParameterNames.push_back("Rtabmap/DetectorStrategy");
oldParameterNames.push_back("RGBD/ScanMatchingSize");
oldParameterNames.push_back("RGBD/LocalLoopDetectionRadius");
oldParameterNames.push_back("RGBD/ToroIterations");
for(std::list<std::string>::iterator iter=oldParameterNames.begin(); iter!=oldParameterNames.end(); ++iter)
{
std::string vStr;
@@ -288,6 +289,12 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
Parameters::kRGBDLocalRadius().c_str());
parameters.at(Parameters::kRGBDLocalRadius())= vStr;
}
else if(iter->compare("RGBD/ToroIterations") == 0)
{
ROS_WARN("Parameter name changed: RGBD/ToroIterations -> %s. Please update your launch file accordingly.",
Parameters::kRGBDOptimizeIterations().c_str());
parameters.at(Parameters::kRGBDOptimizeIterations())= vStr;
}
}
}
@@ -363,16 +370,16 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize, stereoApproxSync);
int toroIterations = 0;
Parameters::parse(parameters, Parameters::kRGBDToroIterations(), toroIterations);
if(publishTf && toroIterations != 0)
int optimizeIterations = 0;
Parameters::parse(parameters, Parameters::kRGBDOptimizeIterations(), optimizeIterations);
if(publishTf && optimizeIterations != 0)
{
transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay));
}
else if(publishTf)
{
UWARN("Graph optimization is disabled (%s=0), the tf between frame \"%s\" and odometry frame will not be published. You can safely ignore this warning if you are using map_optimizer node.",
Parameters::kRGBDToroIterations().c_str(), mapFrameId_.c_str());
Parameters::kRGBDOptimizeIterations().c_str(), mapFrameId_.c_str());
}
}
src/MapOptimizerNode.cpp
+11 -9
View File
@@ -218,15 +218,17 @@ public:
Transform mapCorrection = Transform::getIdentity();
if(poses.size() > 1 && constraints.size() > 0)
{
if(optimizeFromLastNode_)
{
std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(constraints, poses.rbegin()->first);
rtabmap::graph::optimizeTOROGraph(depthGraph, poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
}
else
{
rtabmap::graph::optimizeTOROGraph(poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
}
graph::TOROOptimizer optimizer(iterations_, false, ignoreVariance_);
int fromId = optimizeFromLastNode_?poses.rbegin()->first:poses.begin()->first;
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
optimizer.getConnectedGraph(
fromId,
poses,
constraints,
posesOut,
linksOut);
optimizedPoses = optimizer.optimize(fromId, posesOut, linksOut);
mapToOdomMutex_.lock();
mapCorrection = optimizedPoses.at(poses.rbegin()->first) * poses.rbegin()->second.inverse();