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 OdomInfo msg with icpCorrespondences. rtabmapviz: fixed odom variance stats when only subscribed to odom_info.

Browse Source
This commit is contained in:
matlabbe
2020-03-28 18:51:38 -04:00
parent aced8a8c93
commit af216ebb71
3 changed files with 56 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/OdomInfo.msg
+1
View File
@@ -8,6 +8,7 @@ float32 icpInliersRatio
float32 icpRotation
float32 icpTranslation
float32 icpStructuralComplexity
int32 icpCorrespondences
float64[36] covariance
int32 features
int32 localMapSize
src/GuiWrapper.cpp
+36
View File
@@ -476,6 +476,18 @@ void GuiWrapper::commonDepthCallback(
covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->twist.covariance.data()).clone();
}
}
else if(odomInfoMsg.get() && odomInfoMsg->covariance.size() == 36)
{
if(odomInfoMsg->covariance[0] != 0 &&
odomInfoMsg->covariance[7] != 0 &&
odomInfoMsg->covariance[14] != 0 &&
odomInfoMsg->covariance[21] != 0 &&
odomInfoMsg->covariance[28] != 0 &&
odomInfoMsg->covariance[35] != 0)
{
covariance = cv::Mat(6,6,CV_64FC1,(void*)odomInfoMsg->covariance.data()).clone();
}
}
if(odomHeader.frame_id.empty())
{
ROS_ERROR("Odometry frame not set!?");
@@ -628,6 +640,18 @@ void GuiWrapper::commonStereoCallback(
covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->twist.covariance.data()).clone();
}
}
else if(odomInfoMsg.get() && odomInfoMsg->covariance.size() == 36)
{
if(odomInfoMsg->covariance[0] != 0 &&
odomInfoMsg->covariance[7] != 0 &&
odomInfoMsg->covariance[14] != 0 &&
odomInfoMsg->covariance[21] != 0 &&
odomInfoMsg->covariance[28] != 0 &&
odomInfoMsg->covariance[35] != 0)
{
covariance = cv::Mat(6,6,CV_64FC1,(void*)odomInfoMsg->covariance.data()).clone();
}
}
if(odomHeader.frame_id.empty())
{
ROS_ERROR("Odometry frame not set!?");
@@ -776,6 +800,18 @@ void GuiWrapper::commonLaserScanCallback(
covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->twist.covariance.data()).clone();
}
}
else if(odomInfoMsg.get() && odomInfoMsg->covariance.size() == 36)
{
if(odomInfoMsg->covariance[0] != 0 &&
odomInfoMsg->covariance[7] != 0 &&
odomInfoMsg->covariance[14] != 0 &&
odomInfoMsg->covariance[21] != 0 &&
odomInfoMsg->covariance[28] != 0 &&
odomInfoMsg->covariance[35] != 0)
{
covariance = cv::Mat(6,6,CV_64FC1,(void*)odomInfoMsg->covariance.data()).clone();
}
}
if(odomHeader.frame_id.empty())
{
ROS_ERROR("Odometry frame not set!?");
src/MsgConversion.cpp
+19 -2
View File
@@ -1136,6 +1136,7 @@ std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo &
stats.insert(std::make_pair("Odometry/ICPRotation/rad", info.reg.icpRotation));
stats.insert(std::make_pair("Odometry/ICPTranslation/m", info.reg.icpTranslation));
stats.insert(std::make_pair("Odometry/ICPStructuralComplexity/", info.reg.icpStructuralComplexity));
stats.insert(std::make_pair("Odometry/ICPCorrespondences/", info.reg.icpCorrespondences));
stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)info.reg.covariance.at<double>(0,0))));
stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)info.reg.covariance.at<double>(5,5))));
stats.insert(std::make_pair("Odometry/VarianceLin/", (float)info.reg.covariance.at<double>(0,0)));
@@ -1150,11 +1151,25 @@ std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo &
stats.insert(std::make_pair("Odometry/LocalBundleTime/ms", info.localBundleTime*1000.0f));
stats.insert(std::make_pair("Odometry/KeyFrameAdded/", info.keyFrameAdded?1.0f:0.0f));
stats.insert(std::make_pair("Odometry/Interval/ms", (float)info.interval));
float dist = info.transform.getNorm();
float speed = 0.0f;
if(info.interval>0.0)
speed = info.transform.x()/info.interval*3.6;
stats.insert(std::make_pair("Odometry/Speed/kph", speed));
speed = dist/info.interval;
stats.insert(std::make_pair("Odometry/T/m", dist));
stats.insert(std::make_pair("Odometry/Speed/kph", speed*3.6));
stats.insert(std::make_pair("Odometry/Speed/mph", speed*2.237));
stats.insert(std::make_pair("Odometry/Speed/mps", speed));
stats.insert(std::make_pair("Odometry/Distance/m", info.distanceTravelled));
if(!info.transformGroundTruth.isNull())
{
dist = info.transformGroundTruth.getNorm();
if(info.interval>0.0)
speed = dist/info.interval;
stats.insert(std::make_pair("Odometry/TG/m", dist));
stats.insert(std::make_pair("Odometry/SpeedG/kph", speed*3.6));
stats.insert(std::make_pair("Odometry/SpeedG/mph", speed*2.237));
stats.insert(std::make_pair("Odometry/SpeedG/mps", speed));
}
return stats;
}
@@ -1169,6 +1184,7 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
info.reg.icpRotation = msg.icpRotation;
info.reg.icpTranslation = msg.icpTranslation;
info.reg.icpStructuralComplexity = msg.icpStructuralComplexity;
info.reg.icpCorrespondences = msg.icpCorrespondences;
info.reg.covariance = cv::Mat(6,6,CV_64FC1, (void*)msg.covariance.data()).clone();
info.features = msg.features;
info.localMapSize = msg.localMapSize;
@@ -1225,6 +1241,7 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
msg.icpRotation = info.reg.icpRotation;
msg.icpTranslation = info.reg.icpTranslation;
msg.icpStructuralComplexity = info.reg.icpStructuralComplexity;
msg.icpCorrespondences = info.reg.icpCorrespondences;
if(info.reg.covariance.type() == CV_64FC1 && info.reg.covariance.cols == 6 && info.reg.covariance.rows == 6)
{
memcpy(msg.covariance.data(), info.reg.covariance.data, 36*sizeof(double));