i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-12 20:49:50 +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/MsgConversion.cpp
+1 -1
View File
@@ -878,7 +878,7 @@ void cameraModelToROS(
memcpy(camInfo.K.elems, model.K_raw().data, 9*sizeof(double)); memcpy(camInfo.K.elems, model.K_raw().data, 9*sizeof(double));
} }
if(camInfo.D.size() == 6) if(model.D_raw().total() == 6)
{ {
camInfo.D = std::vector<double>(4); camInfo.D = std::vector<double>(4);
camInfo.D[0] = model.D_raw().at<double>(0,0); camInfo.D[0] = model.D_raw().at<double>(0,0);
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 depthImage16Pub_ = it.advertise("image_raw", 1); // 16 bits unsigned in mm
depthImage32Pub_ = it.advertise("image", 1); // 32 bits float in meters depthImage32Pub_ = it.advertise("image", 1); // 32 bits float in meters
pointCloudTransformedPub_ = nh.advertise<sensor_msgs::PointCloud2>(nh.resolveName("cloud")+"_transformed", 1); 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) if(approx)
{ {
@@ -182,12 +183,15 @@ private:
rtabmap::Transform localTransform = cloudDisplacement*cloudToCamera; rtabmap::Transform localTransform = cloudDisplacement*cloudToCamera;
rtabmap::CameraModel model = rtabmap_ros::cameraModelFromROS(*cameraInfoMsg, localTransform); rtabmap::CameraModel model = rtabmap_ros::cameraModelFromROS(*cameraInfoMsg, localTransform);
sensor_msgs::CameraInfo cameraInfoMsgOut = *cameraInfoMsg;
if(decimation_ > 1) if(decimation_ > 1)
{ {
if(model.imageWidth()%decimation_ == 0 && model.imageHeight()%decimation_ == 0) 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 else
{ {
@@ -257,12 +261,18 @@ private:
cloudStamp, pointCloud2Msg->header.stamp.toSec(), cloudStamp, pointCloud2Msg->header.stamp.toSec(),
infoStamp, cameraInfoMsg->header.stamp.toSec()); infoStamp, cameraInfoMsg->header.stamp.toSec());
} }
if(cameraInfoPub_.getNumSubscribers())
{
rtabmap_ros::cameraModelToROS(model, cameraInfoMsgOut);
}
} }
} }
private: private:
image_transport::Publisher depthImage16Pub_; image_transport::Publisher depthImage16Pub_;
image_transport::Publisher depthImage32Pub_; image_transport::Publisher depthImage32Pub_;
ros::Publisher cameraInfoPub_;
ros::Publisher pointCloudTransformedPub_; ros::Publisher pointCloudTransformedPub_;
message_filters::Subscriber<sensor_msgs::PointCloud2> pointCloudSub_; message_filters::Subscriber<sensor_msgs::PointCloud2> pointCloudSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_; message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;