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

rtabmapviz: using twist covariance instead of pose covariance

Browse Source
This commit is contained in:
matlabbe
2016-08-05 14:22:45 -04:00
parent 30ed81a32c
commit 8ad683736b
2 changed files with 17 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+1 -1
View File
@@ -681,7 +681,7 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
// Only update variance if odom is not null // Only update variance if odom is not null
if(!odom.isNull()) if(!odom.isNull())
{ {
// using MIN in case of 3DoF mapping (maybe not parameters are set, except x and yaw for the twist) // using MIN in case of 3DoF mapping (maybe no parameters are set, except x and yaw for the twist)
float transVariance = uMax3(odomMsg->twist.covariance[0], MIN(odomMsg->twist.covariance[7], BAD_COVARIANCE), MIN(odomMsg->twist.covariance[14], BAD_COVARIANCE)); float transVariance = uMax3(odomMsg->twist.covariance[0], MIN(odomMsg->twist.covariance[7], BAD_COVARIANCE), MIN(odomMsg->twist.covariance[14], BAD_COVARIANCE));
float rotVariance = uMax3(MIN(odomMsg->twist.covariance[21],BAD_COVARIANCE), MIN(odomMsg->twist.covariance[28], BAD_COVARIANCE), odomMsg->twist.covariance[35]); float rotVariance = uMax3(MIN(odomMsg->twist.covariance[21],BAD_COVARIANCE), MIN(odomMsg->twist.covariance[28], BAD_COVARIANCE), odomMsg->twist.covariance[35]);
if(uIsFinite(rotVariance) && rotVariance > rotVariance_) if(uIsFinite(rotVariance) && rotVariance > rotVariance_)
src/GuiWrapper.cpp
+16 -16
View File
@@ -566,15 +566,15 @@ void GuiWrapper::commonDepthCallback(
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomMsg.get()) if(odomMsg.get())
{ {
UASSERT(odomMsg->pose.covariance.size() == 36); UASSERT(odomMsg->twist.covariance.size() == 36);
if(!(odomMsg->pose.covariance[0] == 0 && if(odomMsg->twist.covariance[0] != 0 &&
odomMsg->pose.covariance[7] == 0 && odomMsg->twist.covariance[7] != 0 &&
odomMsg->pose.covariance[14] == 0 && odomMsg->twist.covariance[14] != 0 &&
odomMsg->pose.covariance[21] == 0 && odomMsg->twist.covariance[21] != 0 &&
odomMsg->pose.covariance[28] == 0 && odomMsg->twist.covariance[28] != 0 &&
odomMsg->pose.covariance[35] == 0)) odomMsg->twist.covariance[35] != 0)
{ {
covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->pose.covariance.data()).clone(); covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->twist.covariance.data()).clone();
} }
} }
if(odomHeader.frame_id.empty()) if(odomHeader.frame_id.empty())
@@ -814,15 +814,15 @@ void GuiWrapper::commonStereoCallback(
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomMsg.get()) if(odomMsg.get())
{ {
UASSERT(odomMsg->pose.covariance.size() == 36); UASSERT(odomMsg->twist.covariance.size() == 36);
if(!(odomMsg->pose.covariance[0] == 0 && if(odomMsg->twist.covariance[0] != 0 &&
odomMsg->pose.covariance[7] == 0 && odomMsg->twist.covariance[7] != 0 &&
odomMsg->pose.covariance[14] == 0 && odomMsg->twist.covariance[14] != 0 &&
odomMsg->pose.covariance[21] == 0 && odomMsg->twist.covariance[21] != 0 &&
odomMsg->pose.covariance[28] == 0 && odomMsg->twist.covariance[28] != 0 &&
odomMsg->pose.covariance[35] == 0)) odomMsg->twist.covariance[35] != 0)
{ {
covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->pose.covariance.data()).clone(); covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->twist.covariance.data()).clone();
} }
} }
if(odomHeader.frame_id.empty()) if(odomHeader.frame_id.empty())