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

rtabmap node: added support for odom covariance with z=0, roll=0 and pitch=0 when Reg/Force3DoF is true. Fixed Rtabmap/ImagesAlreadyRectified and Grid/GlobalMaxNodes not modifed on rtabmap_ros side when parameters are dynamically changed.

Browse Source
This commit is contained in:
matlabbe
2021-02-18 18:31:00 -05:00
parent 95206c684a
commit d11e8cac41
1 changed files with 35 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+35 -1
View File
@@ -121,6 +121,7 @@ CoreWrapper::CoreWrapper() :
createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()), createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
maxMappingNodes_(Parameters::defaultGridGlobalMaxNodes()), maxMappingNodes_(Parameters::defaultGridGlobalMaxNodes()),
alreadyRectifiedImages_(Parameters::defaultRtabmapImagesAlreadyRectified()), alreadyRectifiedImages_(Parameters::defaultRtabmapImagesAlreadyRectified()),
twoDMapping_(Parameters::defaultRegForce3DoF()),
previousStamp_(0), previousStamp_(0),
mbClient_(0) mbClient_(0)
{ {
@@ -581,6 +582,10 @@ void CoreWrapper::onInit()
{ {
Parameters::parse(parameters_, Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectifiedImages_); Parameters::parse(parameters_, Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectifiedImages_);
} }
if(parameters_.find(Parameters::kRegForce3DoF()) != parameters_.end())
{
Parameters::parse(parameters_, Parameters::kRegForce3DoF(), twoDMapping_);
}
if(paused_) if(paused_)
{ {
@@ -1227,7 +1232,7 @@ void CoreWrapper::commonDepthCallbackImpl(
genScanMaxDepth_, genScanMaxDepth_,
genScanMinDepth_); genScanMinDepth_);
genMaxScanPts += depth.cols; genMaxScanPts += depth.cols;
scan = LaserScan(rtabmap::util3d::laserScan2dFromPointCloud(*scanCloud2d), 0, genScanMaxDepth_, LaserScan::kXY); scan = LaserScan(rtabmap::util3d::laserScan2dFromPointCloud(*scanCloud2d), 0, genScanMaxDepth_);
} }
else if(!scan2dMsg.ranges.empty()) else if(!scan2dMsg.ranges.empty())
{ {
@@ -1856,6 +1861,13 @@ void CoreWrapper::process(
covariance.at<double>(5,5) = odomDefaultAngVariance_; covariance.at<double>(5,5) = odomDefaultAngVariance_;
} }
} }
else if(twoDMapping_)
{
// 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>(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;
}
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));
Transform gt; Transform gt;
@@ -2025,6 +2037,13 @@ void CoreWrapper::process(
covariance.at<double>(5,5) = odomDefaultAngVariance_; covariance.at<double>(5,5) = odomDefaultAngVariance_;
} }
} }
else if(twoDMapping_)
{
// 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>(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;
}
std::map<std::string, float> externalStats; std::map<std::string, float> externalStats;
std::vector<float> odomVelocity; std::vector<float> odomVelocity;
@@ -2653,6 +2672,21 @@ bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Emp
createIntermediateNodes_ = uStr2Bool(parameters_.at(Parameters::kRtabmapCreateIntermediateNodes())); createIntermediateNodes_ = uStr2Bool(parameters_.at(Parameters::kRtabmapCreateIntermediateNodes()));
NODELET_INFO("Create intermediate nodes = %s", createIntermediateNodes_?"true":"false"); NODELET_INFO("Create intermediate nodes = %s", createIntermediateNodes_?"true":"false");
} }
if(parameters_.find(Parameters::kGridGlobalMaxNodes()) != parameters_.end())
{
maxMappingNodes_ = uStr2Int(parameters_.at(Parameters::kGridGlobalMaxNodes()));
NODELET_INFO("Max mapping nodes = %d", maxMappingNodes_);
}
if(parameters_.find(Parameters::kRtabmapImagesAlreadyRectified()) != parameters_.end())
{
alreadyRectifiedImages_ = uStr2Bool(parameters_.at(Parameters::kRtabmapImagesAlreadyRectified()));
NODELET_INFO("Already rectified images = %s", alreadyRectifiedImages_?"true":"false");
}
if(parameters_.find(Parameters::kRegForce3DoF()) != parameters_.end())
{
twoDMapping_= uStr2Bool(parameters_.at(Parameters::kRegForce3DoF()));
NODELET_INFO("2D mapping = %s", twoDMapping_?"true":"false");
}
rtabmap_.parseParameters(parameters_); rtabmap_.parseParameters(parameters_);
mapsManager_.setParameters(parameters_); mapsManager_.setParameters(parameters_);
return true; return true;