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

pointcloud_to_depthimage: fixed moving relative transformation

Browse Source
This commit is contained in:
matlabbe
2017-09-09 22:10:13 -04:00
parent f4978e4b92
commit b772fce84b
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/pointcloud_to_depthimage.cpp
+3 -2
View File
@@ -58,6 +58,7 @@ class PointCloudToDepthImage : public nodelet::Nodelet
{ {
public: public:
PointCloudToDepthImage() : PointCloudToDepthImage() :
listener_(0),
waitForTransform_(0.1), waitForTransform_(0.1),
fillHolesSize_ (0), fillHolesSize_ (0),
fillHolesError_(0.1), fillHolesError_(0.1),
@@ -144,7 +145,7 @@ private:
if(!fixedFrameId_.empty()) if(!fixedFrameId_.empty())
{ {
// approx sync // approx sync
rtabmap::Transform cloudDisplacement = rtabmap_ros::getTransform( cloudDisplacement = rtabmap_ros::getTransform(
pointCloud2Msg->header.frame_id, pointCloud2Msg->header.frame_id,
fixedFrameId_, fixedFrameId_,
pointCloud2Msg->header.stamp, pointCloud2Msg->header.stamp,
@@ -170,7 +171,7 @@ private:
return; return;
} }
rtabmap::Transform localTransform = cloudToCamera*cloudDisplacement; rtabmap::Transform localTransform = cloudDisplacement.inverse()*cloudToCamera;
rtabmap::CameraModel model = rtabmap_ros::cameraModelFromROS(*cameraInfoMsg, localTransform); rtabmap::CameraModel model = rtabmap_ros::cameraModelFromROS(*cameraInfoMsg, localTransform);