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

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/CoreWrapper.cpp
+5 -6
View File
@@ -763,16 +763,15 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
covariance = cv::Mat(6,6,CV_64FC1, (void*)odomMsg->twist.covariance.data()).clone();
}
if(uIsFinite(covariance.at<double>(0,0)) && covariance.at<double>(0,0) != 1.0 && covariance.at<double>(0,0)>0.0)
if(uIsFinite(covariance.at<double>(0,0)) &&
covariance.at<double>(0,0) != 1.0 &&
covariance.at<double>(0,0)>0.0)
{
if(covariance_.empty())
// Use largest covariance error (to be independent of the odometry frame rate)
if(covariance_.empty() || covariance.at<double>(0,0) > covariance_.at<double>(0,0))
{
covariance_ = covariance;
}
else
{
covariance_ += covariance;
}
}
}
src/GuiWrapper.cpp
+8 -2
View File
@@ -302,7 +302,10 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
if(!cameraNodeName_.empty())
{
std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause true", cameraNodeName_.c_str());
system(str.c_str());
if(system(str.c_str()) !=0)
{
ROS_ERROR("Command \"%s\" returned non zero value.", str.c_str());
}
}
// Pause visual_odometry
@@ -329,7 +332,10 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
if(!cameraNodeName_.empty())
{
std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause false", cameraNodeName_.c_str());
system(str.c_str());
if(system(str.c_str()) !=0)
{
ROS_ERROR("Command \"%s\" returned non zero value.", str.c_str());
}
}
}
else if(cmd == rtabmap::RtabmapEventCmd::kCmdTriggerNewMap)
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;