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

Check inf values in odom topic (convert them to 1 if Reg/Force3DoF is true for convenience instead of asserting). Added support for size of tags.

Browse Source
This commit is contained in:
matlabbe
2021-07-30 09:46:22 -04:00
parent b311c6f21a
commit d7a33b86ed
4 changed files with 20 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+9 -9
View File
@@ -1561,7 +1561,7 @@ void userDataToROS(const cv::Mat & data, rtabmap_ros::UserData & dataMsg, bool c
}
rtabmap::Landmarks landmarksFromROS(
const std::map<int, geometry_msgs::PoseWithCovarianceStamped> & tags,
const std::map<int, std::pair<geometry_msgs::PoseWithCovarianceStamped, float> > & tags,
const std::string & frameId,
const std::string & odomFrameId,
const ros::Time & odomStamp,
@@ -1572,7 +1572,7 @@ rtabmap::Landmarks landmarksFromROS(
{
//tag detections
rtabmap::Landmarks landmarks;
for(std::map<int, geometry_msgs::PoseWithCovarianceStamped>::const_iterator iter=tags.begin(); iter!=tags.end(); ++iter)
for(std::map<int, std::pair<geometry_msgs::PoseWithCovarianceStamped, float> >::const_iterator iter=tags.begin(); iter!=tags.end(); ++iter)
{
if(iter->first <=0)
{
@@ -1581,19 +1581,19 @@ rtabmap::Landmarks landmarksFromROS(
}
rtabmap::Transform baseToCamera = rtabmap_ros::getTransform(
frameId,
iter->second.header.frame_id,
iter->second.header.stamp,
iter->second.first.header.frame_id,
iter->second.first.header.stamp,
listener,
waitForTransform);
if(baseToCamera.isNull())
{
ROS_ERROR("Cannot transform tag pose from \"%s\" frame to \"%s\" frame!",
iter->second.header.frame_id.c_str(), frameId.c_str());
iter->second.first.header.frame_id.c_str(), frameId.c_str());
continue;
}
rtabmap::Transform baseToTag = baseToCamera * transformFromPoseMsg(iter->second.pose.pose);
rtabmap::Transform baseToTag = baseToCamera * transformFromPoseMsg(iter->second.first.pose.pose);
if(!baseToTag.isNull())
{
@@ -1601,7 +1601,7 @@ rtabmap::Landmarks landmarksFromROS(
rtabmap::Transform correction = rtabmap_ros::getTransform(
frameId,
odomFrameId,
iter->second.header.stamp,
iter->second.first.header.stamp,
odomStamp,
listener,
waitForTransform);
@@ -1615,14 +1615,14 @@ rtabmap::Landmarks landmarksFromROS(
"If odometry is small since it received the tag pose and "
"covariance is large, this should not be a problem.");
}
cv::Mat covariance = cv::Mat(6,6, CV_64FC1, (void*)iter->second.pose.covariance.data()).clone();
cv::Mat covariance = cv::Mat(6,6, CV_64FC1, (void*)iter->second.first.pose.covariance.data()).clone();
if(covariance.empty() || !uIsFinite(covariance.at<double>(0,0)) || covariance.at<double>(0,0)<=0.0f)
{
covariance = cv::Mat::eye(6,6,CV_64FC1);
covariance(cv::Range(0,3), cv::Range(0,3)) *= defaultLinVariance;
covariance(cv::Range(3,6), cv::Range(3,6)) *= defaultAngVariance;
}
landmarks.insert(std::make_pair(iter->first, rtabmap::Landmark(iter->first, baseToTag, covariance)));
landmarks.insert(std::make_pair(iter->first, rtabmap::Landmark(iter->first, iter->second.second, baseToTag, covariance)));
}
}
return landmarks;