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

pointcloud_to_depthimage: republish scaled camera_info in case decimation parameter has been used.

Browse Source
This commit is contained in:
matlabbe
2022-03-06 17:48:47 -05:00
parent 05a1be02a4
commit ec9d32bebd
2 changed files with 13 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/pointcloud_to_depthimage.cpp
+12 -2
View File
@@ -123,6 +123,7 @@ private:
depthImage16Pub_ = it.advertise("image_raw", 1); // 16 bits unsigned in mm
depthImage32Pub_ = it.advertise("image", 1); // 32 bits float in meters
pointCloudTransformedPub_ = nh.advertise<sensor_msgs::PointCloud2>(nh.resolveName("cloud")+"_transformed", 1);
cameraInfoPub_ = nh.advertise<sensor_msgs::PointCloud2>(nh.resolveName("camera_info")+"_depth", 1);
if(approx)
{
@@ -182,12 +183,15 @@ private:
rtabmap::Transform localTransform = cloudDisplacement*cloudToCamera;
rtabmap::CameraModel model = rtabmap_ros::cameraModelFromROS(*cameraInfoMsg, localTransform);
sensor_msgs::CameraInfo cameraInfoMsgOut = *cameraInfoMsg;
if(decimation_ > 1)
{
if(model.imageWidth()%decimation_ == 0 && model.imageHeight()%decimation_ == 0)
{
model = model.scaled(1.0f/float(decimation_));
float scale = 1.0f/float(decimation_);
model = model.scaled(scale);
rtabmap_ros::cameraModelToROS(model, cameraInfoMsgOut);
}
else
{
@@ -257,12 +261,18 @@ private:
cloudStamp, pointCloud2Msg->header.stamp.toSec(),
infoStamp, cameraInfoMsg->header.stamp.toSec());
}
if(cameraInfoPub_.getNumSubscribers())
{
rtabmap_ros::cameraModelToROS(model, cameraInfoMsgOut);
}
}
}
private:
image_transport::Publisher depthImage16Pub_;
image_transport::Publisher depthImage32Pub_;
ros::Publisher cameraInfoPub_;
ros::Publisher pointCloudTransformedPub_;
message_filters::Subscriber<sensor_msgs::PointCloud2> pointCloudSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;