i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-11 03:59:53 +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
include/rtabmap_ros/CoreWrapper.h
+1 -1
View File
@@ -356,7 +356,7 @@ private:
ros::Subscriber gpsFixAsyncSub_; ros::Subscriber gpsFixAsyncSub_;
rtabmap::GPS gps_; rtabmap::GPS gps_;
ros::Subscriber tagDetectionsSub_; ros::Subscriber tagDetectionsSub_;
std::map<int, geometry_msgs::PoseWithCovarianceStamped> tags_; std::map<int, std::pair<geometry_msgs::PoseWithCovarianceStamped, float> > tags_; // id, <pose, size>
ros::Subscriber imuSub_; ros::Subscriber imuSub_;
std::map<double, rtabmap::Transform> imus_; std::map<double, rtabmap::Transform> imus_;
std::string imuFrameId_; std::string imuFrameId_;
include/rtabmap_ros/MsgConversion.h
+1 -1
View File
@@ -175,7 +175,7 @@ cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg);
void userDataToROS(const cv::Mat & data, rtabmap_ros::UserData & dataMsg, bool compress); void userDataToROS(const cv::Mat & data, rtabmap_ros::UserData & dataMsg, bool compress);
rtabmap::Landmarks landmarksFromROS( 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 & frameId,
const std::string & odomFrameId, const std::string & odomFrameId,
const ros::Time & odomStamp, const ros::Time & odomStamp,
src/CoreWrapper.cpp
+9 -7
View File
@@ -1912,9 +1912,9 @@ void CoreWrapper::process(
else if(twoDMapping_) else if(twoDMapping_)
{ {
// If 2d mapping, make sure all diagonal values of the covariance that even not used are not null. // If 2d mapping, make sure all diagonal values of the covariance that even not used are not null.
covariance.at<double>(2,2) = covariance.at<double>(2,2)!=0?covariance.at<double>(2,2):1; covariance.at<double>(2,2) = uIsFinite(covariance.at<double>(2,2)) && covariance.at<double>(2,2)!=0?covariance.at<double>(2,2):1;
covariance.at<double>(3,3) = covariance.at<double>(3,3)!=0?covariance.at<double>(3,3):1; covariance.at<double>(3,3) = uIsFinite(covariance.at<double>(3,3)) && covariance.at<double>(3,3)!=0?covariance.at<double>(3,3):1;
covariance.at<double>(4,4) = covariance.at<double>(4,4)!=0?covariance.at<double>(4,4):1; covariance.at<double>(4,4) = uIsFinite(covariance.at<double>(4,4)) && covariance.at<double>(4,4)!=0?covariance.at<double>(4,4):1;
} }
SensorData interData(cv::Mat(), cv::Mat(), CameraModel(), -1, rtabmap_ros::timestampFromROS(iter->first.header.stamp)); SensorData interData(cv::Mat(), cv::Mat(), CameraModel(), -1, rtabmap_ros::timestampFromROS(iter->first.header.stamp));
@@ -2088,9 +2088,9 @@ void CoreWrapper::process(
else if(twoDMapping_) else if(twoDMapping_)
{ {
// If 2d mapping, make sure all diagonal values of the covariance that even not used are not null. // If 2d mapping, make sure all diagonal values of the covariance that even not used are not null.
covariance.at<double>(2,2) = covariance.at<double>(2,2)!=0?covariance.at<double>(2,2):1; covariance.at<double>(2,2) = uIsFinite(covariance.at<double>(2,2)) && covariance.at<double>(2,2)!=0?covariance.at<double>(2,2):1;
covariance.at<double>(3,3) = covariance.at<double>(3,3)!=0?covariance.at<double>(3,3):1; covariance.at<double>(3,3) = uIsFinite(covariance.at<double>(3,3)) && covariance.at<double>(3,3)!=0?covariance.at<double>(3,3):1;
covariance.at<double>(4,4) = covariance.at<double>(4,4)!=0?covariance.at<double>(4,4):1; covariance.at<double>(4,4) = uIsFinite(covariance.at<double>(4,4)) && covariance.at<double>(4,4)!=0?covariance.at<double>(4,4):1;
} }
std::map<std::string, float> externalStats; std::map<std::string, float> externalStats;
@@ -2454,7 +2454,9 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetecti
warned = true; warned = true;
} }
} }
uInsert(tags_, std::make_pair(tagDetections.detections[i].id[0], p)); uInsert(tags_,
std::make_pair(tagDetections.detections[i].id[0],
std::make_pair(p, tagDetections.detections[i].size.size()==1?(float)tagDetections.detections[i].size[0]:0.0f)));
} }
} }
} }
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( 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 & frameId,
const std::string & odomFrameId, const std::string & odomFrameId,
const ros::Time & odomStamp, const ros::Time & odomStamp,
@@ -1572,7 +1572,7 @@ rtabmap::Landmarks landmarksFromROS(
{ {
//tag detections //tag detections
rtabmap::Landmarks landmarks; 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) if(iter->first <=0)
{ {
@@ -1581,19 +1581,19 @@ rtabmap::Landmarks landmarksFromROS(
} }
rtabmap::Transform baseToCamera = rtabmap_ros::getTransform( rtabmap::Transform baseToCamera = rtabmap_ros::getTransform(
frameId, frameId,
iter->second.header.frame_id, iter->second.first.header.frame_id,
iter->second.header.stamp, iter->second.first.header.stamp,
listener, listener,
waitForTransform); waitForTransform);
if(baseToCamera.isNull()) if(baseToCamera.isNull())
{ {
ROS_ERROR("Cannot transform tag pose from \"%s\" frame to \"%s\" frame!", 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; continue;
} }
rtabmap::Transform baseToTag = baseToCamera * transformFromPoseMsg(iter->second.pose.pose); rtabmap::Transform baseToTag = baseToCamera * transformFromPoseMsg(iter->second.first.pose.pose);
if(!baseToTag.isNull()) if(!baseToTag.isNull())
{ {
@@ -1601,7 +1601,7 @@ rtabmap::Landmarks landmarksFromROS(
rtabmap::Transform correction = rtabmap_ros::getTransform( rtabmap::Transform correction = rtabmap_ros::getTransform(
frameId, frameId,
odomFrameId, odomFrameId,
iter->second.header.stamp, iter->second.first.header.stamp,
odomStamp, odomStamp,
listener, listener,
waitForTransform); waitForTransform);
@@ -1615,14 +1615,14 @@ rtabmap::Landmarks landmarksFromROS(
"If odometry is small since it received the tag pose and " "If odometry is small since it received the tag pose and "
"covariance is large, this should not be a problem."); "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) 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::Mat::eye(6,6,CV_64FC1);
covariance(cv::Range(0,3), cv::Range(0,3)) *= defaultLinVariance; covariance(cv::Range(0,3), cv::Range(0,3)) *= defaultLinVariance;
covariance(cv::Range(3,6), cv::Range(3,6)) *= defaultAngVariance; 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; return landmarks;