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

obstacles_detection: fixed transformation back of segmented clouds to topic's frame

Browse Source
This commit is contained in:
matlabbe
2017-07-27 16:29:43 -04:00
parent 936e5ad093
commit dcfbab67a9
1 changed files with 21 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/obstacles_detection.cpp
+21 -8
View File
@@ -50,7 +50,8 @@ class ObstaclesDetection : public nodelet::Nodelet
public:
ObstaclesDetection() :
frameId_("base_link"),
waitForTransform_(false)
waitForTransform_(false),
mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection())
{}
virtual ~ObstaclesDetection()
@@ -194,7 +195,8 @@ private:
parameterMoved(pnh, "normal_k", rtabmap::Parameters::kGridNormalK(), parameters);
UASSERT(uContains(parameters, rtabmap::Parameters::kGridMapFrameProjection()));
if(uStr2Bool(parameters.at(rtabmap::Parameters::kGridMapFrameProjection())) && mapFrameId_.empty())
mapFrameProjection_ = uStr2Bool(parameters.at(rtabmap::Parameters::kGridMapFrameProjection()));
if(mapFrameProjection_ && mapFrameId_.empty())
{
NODELET_ERROR("obstacles_detection: Parameter \"%s\" is true but map_frame_id is not set!", rtabmap::Parameters::kGridMapFrameProjection().c_str());
}
@@ -220,7 +222,7 @@ private:
return;
}
rtabmap::Transform localTransform;
rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
try
{
if(waitForTransform_)
@@ -326,17 +328,27 @@ private:
obstaclesCloudWithoutFlatSurfaces->resize(oi);
}
if(!localTransform.isIdentity())
if(!localTransform.isIdentity() || !pose.isIdentity())
{
//transform back in topic frame
rtabmap::Transform localTransformInv = localTransform.inverse();
//transform back in topic frame for 3d clouds and base frame for 2d clouds
float roll, pitch, yaw;
pose.getEulerAngles(roll, pitch, yaw);
rtabmap::Transform t = rtabmap::Transform(0,0, mapFrameProjection_?pose.z():0, roll, pitch, 0);
if(obstaclesCloudWithoutFlatSurfaces->size() && !pose.isIdentity())
{
obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::transformPointCloud(obstaclesCloudWithoutFlatSurfaces, t.inverse());
}
t = (t*localTransform).inverse();
if(groundCloud->size())
{
groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, localTransformInv);
groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, t);
}
if(obstaclesCloud->size())
{
obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, localTransformInv);
obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, t);
}
}
}
@@ -382,6 +394,7 @@ private:
bool waitForTransform_;
rtabmap::OccupancyGrid grid_;
bool mapFrameProjection_;
tf::TransformListener tfListener_;