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

icp_odometry: Fixed Icp/PointToPlaneK and Icp/PointPlaneRadius not updated when using directly scan_normal_k and scan_normal_radius.

Browse Source
This commit is contained in:
matlabbe
2021-12-30 14:58:25 -05:00
parent 9a3e47e054
commit 0503f1a1f6
1 changed files with 39 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/icp_odometry.cpp
+39 -4
View File
@@ -181,8 +181,8 @@ private:
iter = parameters.find(Parameters::kIcpRangeMin()); iter = parameters.find(Parameters::kIcpRangeMin());
if(iter != parameters.end()) if(iter != parameters.end())
{ {
int value = uStr2Int(iter->second); float value = uStr2Float(iter->second);
if(value > 1) if(value != 0.0f)
{ {
if(!pnh.hasParam("scan_range_min")) if(!pnh.hasParam("scan_range_min"))
{ {
@@ -199,8 +199,8 @@ private:
iter = parameters.find(Parameters::kIcpRangeMax()); iter = parameters.find(Parameters::kIcpRangeMax());
if(iter != parameters.end()) if(iter != parameters.end())
{ {
int value = uStr2Int(iter->second); float value = uStr2Float(iter->second);
if(value > 1) if(value != 0.0f)
{ {
if(!pnh.hasParam("scan_range_max")) if(!pnh.hasParam("scan_range_max"))
{ {
@@ -232,6 +232,11 @@ private:
} }
} }
} }
else if(pnh.hasParam("scan_voxel_size"))
{
NODELET_INFO("IcpOdometry: scan_voxel_size is set (%f), setting %s to 0", scanVoxelSize_, Parameters::kIcpVoxelSize().c_str());
parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), "0"));
}
iter = parameters.find(Parameters::kIcpPointToPlaneK()); iter = parameters.find(Parameters::kIcpPointToPlaneK());
if(iter != parameters.end()) if(iter != parameters.end())
{ {
@@ -243,8 +248,18 @@ private:
ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_k\" for convenience.", iter->second.c_str(), iter->first.c_str()); ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_k\" for convenience.", iter->second.c_str(), iter->first.c_str());
scanNormalK_ = value; scanNormalK_ = value;
} }
else
{
NODELET_INFO("IcpOdometry: scan_normal_k is set (%d), setting %s to same value.", scanNormalK_, Parameters::kIcpPointToPlaneK().c_str());
iter->second = uNumber2Str(scanNormalK_);
}
} }
} }
else if(pnh.hasParam("scan_normal_k"))
{
NODELET_INFO("IcpOdometry: scan_normal_k is set (%d), setting %s to same value.", scanNormalK_, Parameters::kIcpPointToPlaneK().c_str());
parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneK(), uNumber2Str(scanNormalK_)));
}
iter = parameters.find(Parameters::kIcpPointToPlaneRadius()); iter = parameters.find(Parameters::kIcpPointToPlaneRadius());
if(iter != parameters.end()) if(iter != parameters.end())
{ {
@@ -256,8 +271,18 @@ private:
ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_radius\" for convenience.", iter->second.c_str(), iter->first.c_str()); ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_radius\" for convenience.", iter->second.c_str(), iter->first.c_str());
scanNormalRadius_ = value; scanNormalRadius_ = value;
} }
else
{
NODELET_INFO("IcpOdometry: scan_normal_radius is set (%f), setting %s to same value.", scanNormalRadius_, Parameters::kIcpPointToPlaneRadius().c_str());
iter->second = uNumber2Str(scanNormalK_);
}
} }
} }
else if(pnh.hasParam("scan_normal_radius"))
{
NODELET_INFO("IcpOdometry: scan_normal_radius is set (%f), setting %s to same value.", scanNormalRadius_, Parameters::kIcpPointToPlaneRadius().c_str());
parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneRadius(), uNumber2Str(scanNormalRadius_)));
}
iter = parameters.find(Parameters::kIcpPointToPlaneGroundNormalsUp()); iter = parameters.find(Parameters::kIcpPointToPlaneGroundNormalsUp());
if(iter != parameters.end()) if(iter != parameters.end())
{ {
@@ -269,8 +294,18 @@ private:
ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_ground_up\" for convenience.", iter->second.c_str(), iter->first.c_str()); ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_ground_up\" for convenience.", iter->second.c_str(), iter->first.c_str());
scanNormalGroundUp_ = value; scanNormalGroundUp_ = value;
} }
else
{
NODELET_INFO("IcpOdometry: scan_normal_ground_up is set (%f), setting %s to same value.", scanNormalGroundUp_, Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
iter->second = uNumber2Str(scanNormalK_);
}
} }
} }
else if(pnh.hasParam("scan_normal_ground_up"))
{
NODELET_INFO("IcpOdometry: scan_normal_ground_up is set (%f), setting %s to same value.", scanNormalGroundUp_, Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(scanNormalGroundUp_)));
}
} }
void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg) void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg)