i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-06 09:47: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/GuiWrapper.cpp
+16 -16
View File
@@ -566,15 +566,15 @@ void GuiWrapper::commonDepthCallback(
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomMsg.get())
{
UASSERT(odomMsg->pose.covariance.size() == 36);
if(!(odomMsg->pose.covariance[0] == 0 &&
odomMsg->pose.covariance[7] == 0 &&
odomMsg->pose.covariance[14] == 0 &&
odomMsg->pose.covariance[21] == 0 &&
odomMsg->pose.covariance[28] == 0 &&
odomMsg->pose.covariance[35] == 0))
UASSERT(odomMsg->twist.covariance.size() == 36);
if(odomMsg->twist.covariance[0] != 0 &&
odomMsg->twist.covariance[7] != 0 &&
odomMsg->twist.covariance[14] != 0 &&
odomMsg->twist.covariance[21] != 0 &&
odomMsg->twist.covariance[28] != 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())
@@ -814,15 +814,15 @@ void GuiWrapper::commonStereoCallback(
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomMsg.get())
{
UASSERT(odomMsg->pose.covariance.size() == 36);
if(!(odomMsg->pose.covariance[0] == 0 &&
odomMsg->pose.covariance[7] == 0 &&
odomMsg->pose.covariance[14] == 0 &&
odomMsg->pose.covariance[21] == 0 &&
odomMsg->pose.covariance[28] == 0 &&
odomMsg->pose.covariance[35] == 0))
UASSERT(odomMsg->twist.covariance.size() == 36);
if(odomMsg->twist.covariance[0] != 0 &&
odomMsg->twist.covariance[7] != 0 &&
odomMsg->twist.covariance[14] != 0 &&
odomMsg->twist.covariance[21] != 0 &&
odomMsg->twist.covariance[28] != 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())