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

rtabmap: use largest odom covariance instead of summation

Browse Source
This commit is contained in:
matlabbe
2017-07-13 11:39:19 -04:00
parent e944786ee5
commit 7f392f3796
3 changed files with 24 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+11 -7
View File
@@ -356,14 +356,18 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
Transform initialPose = getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp);
if(initialPose.isNull())
{
return;
NODELET_WARN("Ground truth frames \"%s\" -> \"%s\" are set but failed to "
"get them, odometry won't be synchronized with ground truth.",
groundTruthFrameId_.c_str(), groundTruthBaseFrameId_.c_str());
}
else
{
NODELET_INFO( "Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
initialPose.prettyPrint().c_str(),
groundTruthFrameId_.c_str(),
groundTruthBaseFrameId_.c_str());
odometry_->reset(initialPose);
}
NODELET_INFO( "Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
initialPose.prettyPrint().c_str(),
groundTruthFrameId_.c_str(),
groundTruthBaseFrameId_.c_str());
odometry_->reset(initialPose);
}
Transform guess;