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

Added support for parameter GridGlobal/AltitudeDelta

Browse Source
This commit is contained in:
matlabbe
2021-02-21 13:04:08 -05:00
parent 8f3399626a
commit 287d7382bc
2 changed files with 64 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/CoreWrapper.h
+5 -1
View File
@@ -184,6 +184,9 @@ private:
const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1),
const rtabmap::OdometryInfo & odomInfo = rtabmap::OdometryInfo(),
double timeMsgConversion = 0.0);
std::map<int, rtabmap::Transform> filterNodesToAssemble(
const std::map<int, rtabmap::Transform> & nodes,
const rtabmap::Transform & currentPose);
bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -366,7 +369,8 @@ private:
bool odomSensorSync_;
float rate_;
bool createIntermediateNodes_;
int maxMappingNodes_;
int mappingMaxNodes_;
double mappingAltitudeDelta_;
bool alreadyRectifiedImages_;
bool twoDMapping_;
ros::Time previousStamp_;
src/CoreWrapper.cpp
+59 -48
View File
@@ -119,7 +119,8 @@ CoreWrapper::CoreWrapper() :
odomSensorSync_(false),
rate_(Parameters::defaultRtabmapDetectionRate()),
createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
maxMappingNodes_(Parameters::defaultGridGlobalMaxNodes()),
mappingMaxNodes_(Parameters::defaultGridGlobalMaxNodes()),
mappingAltitudeDelta_(Parameters::defaultGridGlobalAltitudeDelta()),
alreadyRectifiedImages_(Parameters::defaultRtabmapImagesAlreadyRectified()),
twoDMapping_(Parameters::defaultRegForce3DoF()),
previousStamp_(0),
@@ -572,10 +573,18 @@ void CoreWrapper::onInit()
}
if(parameters_.find(Parameters::kGridGlobalMaxNodes()) != parameters_.end())
{
Parameters::parse(parameters_, Parameters::kGridGlobalMaxNodes(), maxMappingNodes_);
if(maxMappingNodes_>0)
Parameters::parse(parameters_, Parameters::kGridGlobalMaxNodes(), mappingMaxNodes_);
if(mappingMaxNodes_>0)
{
NODELET_INFO("Max mapping nodes = %d", maxMappingNodes_);
NODELET_INFO("Max mapping nodes = %d", mappingMaxNodes_);
}
}
if(parameters_.find(Parameters::kGridGlobalAltitudeDelta()) != parameters_.end())
{
Parameters::parse(parameters_, Parameters::kGridGlobalAltitudeDelta(), mappingAltitudeDelta_);
if(mappingAltitudeDelta_>0.0)
{
NODELET_INFO("Mapping altitude delta = %f", mappingAltitudeDelta_);
}
}
if(parameters_.find(Parameters::kRtabmapImagesAlreadyRectified()) != parameters_.end())
@@ -2115,14 +2124,10 @@ void CoreWrapper::process(
filteredPoses.insert(std::make_pair(0, mapToOdom_*odom));
}
if(maxMappingNodes_ > 0 && filteredPoses.size()>1)
if((mappingMaxNodes_ > 0 || mappingAltitudeDelta_>0.0) && filteredPoses.size()>1)
{
std::map<int, Transform> nearestPoses;
std::map<int, float> nodes = graph::findNearestNodes(filteredPoses, mapToOdom_*odom, maxMappingNodes_);
for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
{
nearestPoses.insert(*filteredPoses.find(iter->first));
}
std::map<int, Transform> nearestPoses = filterNodesToAssemble(filteredPoses, mapToOdom_*odom);
//add latest/zero and make sure those on a planned path are not filtered
std::set<int> onPath;
if(rtabmap_.getPath().size())
@@ -2279,6 +2284,36 @@ void CoreWrapper::process(
}
}
std::map<int, Transform> CoreWrapper::filterNodesToAssemble(
const std::map<int, Transform> & nodes,
const Transform & currentPose)
{
std::map<int, Transform> output;
if(mappingMaxNodes_ > 0)
{
std::map<int, float> nodesDist = graph::findNearestNodes(nodes, currentPose, mappingMaxNodes_);
for(std::map<int, float>::iterator iter=nodesDist.begin(); iter!=nodesDist.end(); ++iter)
{
if(mappingAltitudeDelta_<=0.0 ||
fabs(nodes.at(iter->first).z()-currentPose.z())<mappingAltitudeDelta_)
{
output.insert(*nodes.find(iter->first));
}
}
}
else // mappingAltitudeDelta_>0.0
{
for(std::map<int, Transform>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
{
if(fabs(iter->second.z()-currentPose.z())<mappingAltitudeDelta_)
{
output.insert(*iter);
}
}
}
return output;
}
void CoreWrapper::userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & dataMsg)
{
if(!paused_)
@@ -2674,8 +2709,13 @@ bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Emp
}
if(parameters_.find(Parameters::kGridGlobalMaxNodes()) != parameters_.end())
{
maxMappingNodes_ = uStr2Int(parameters_.at(Parameters::kGridGlobalMaxNodes()));
NODELET_INFO("Max mapping nodes = %d", maxMappingNodes_);
mappingMaxNodes_ = uStr2Int(parameters_.at(Parameters::kGridGlobalMaxNodes()));
NODELET_INFO("Max mapping nodes = %d", mappingMaxNodes_);
}
if(parameters_.find(Parameters::kGridGlobalAltitudeDelta()) != parameters_.end())
{
mappingAltitudeDelta_ = uStr2Float(parameters_.at(Parameters::kGridGlobalAltitudeDelta()));
NODELET_INFO("Mapping altitude delta = %f", mappingAltitudeDelta_);
}
if(parameters_.find(Parameters::kRtabmapImagesAlreadyRectified()) != parameters_.end())
{
@@ -3161,18 +3201,9 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
if(mapsManager_.hasSubscribers())
{
std::map<int, Transform> filteredPoses(poses.lower_bound(1), poses.end());
if(maxMappingNodes_ > 0 && filteredPoses.size()>1)
if((mappingMaxNodes_ > 0 || mappingAltitudeDelta_>0.0) && filteredPoses.size()>1)
{
std::map<int, Transform> nearestPoses;
std::map<int, float> nodes = graph::findNearestNodes(filteredPoses, filteredPoses.rbegin()->second, maxMappingNodes_);
for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
{
std::map<int, Transform>::iterator pter = filteredPoses.find(iter->first);
if(pter != filteredPoses.end())
{
nearestPoses.insert(*pter);
}
}
std::map<int, Transform> nearestPoses = filterNodesToAssemble(filteredPoses, filteredPoses.rbegin()->second);
}
if(signatures.size())
{
@@ -4038,19 +4069,9 @@ bool CoreWrapper::octomapBinaryCallback(
res.map.header.stamp = ros::Time::now();
std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
if(maxMappingNodes_ > 0 && poses.size()>1)
if((mappingMaxNodes_ > 0 || mappingAltitudeDelta_>0.0) && poses.size()>1)
{
std::map<int, Transform> nearestPoses;
std::map<int, float> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxMappingNodes_);
for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
{
std::map<int, Transform>::iterator pter = poses.find(iter->first);
if(pter != poses.end())
{
nearestPoses.insert(*pter);
}
}
poses = nearestPoses;
poses = filterNodesToAssemble(poses, poses.rbegin()->second);
}
mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true);
@@ -4069,19 +4090,9 @@ bool CoreWrapper::octomapFullCallback(
res.map.header.stamp = ros::Time::now();
std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
if(maxMappingNodes_ > 0 && poses.size()>1)
if((mappingMaxNodes_ > 0 || mappingAltitudeDelta_>0.0) && poses.size()>1)
{
std::map<int, Transform> nearestPoses;
std::map<int, float> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxMappingNodes_);
for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
{
std::map<int, Transform>::iterator pter = poses.find(iter->first);
if(pter != poses.end())
{
nearestPoses.insert(*pter);
}
}
poses = nearestPoses;
poses = filterNodesToAssemble(poses, poses.rbegin()->second);
}
mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true);