Now generates point cloud rgb from the depth image.

130s · Jan 15, 2019 · 08fd374 · 08fd374
1 parent d93cbb5
commit 08fd374
Showing 1 changed file with 7 additions and 6 deletions.
diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp
@@ -691,7 +691,7 @@ void BaseRealSenseNode::setupStreams()
                                 _image[DEPTH].data = (uint8_t*)depth_frame.get_data();
                             }
 
-                            if (_enable_filter && is_frame_arrived.at(COLOR))
+                            if (_enable_filter && is_frame_arrived.at(INFRARGB))
                             {
                                 publishFrame(f, t,
                                              sip,
@@ -1171,7 +1171,7 @@ void BaseRealSenseNode::publishRgbToDepthPCTopic(const ros::Time& t, const std::
 {
     try
     {
-        if (!is_frame_arrived.at(COLOR) || !is_frame_arrived.at(DEPTH))
+        if (!is_frame_arrived.at(INFRARGB) || !is_frame_arrived.at(DEPTH))
         {
             ROS_DEBUG("Skipping publish PC topic! Color or Depth frame didn't arrive.");
             return;
@@ -1214,11 +1214,13 @@ void BaseRealSenseNode::publishRgbToDepthPCTopic(const ros::Time& t, const std::
 
 void BaseRealSenseNode::publishITRTopic(sensor_msgs::PointCloud2& msg_pointcloud, bool colorized_pointcloud)
 {
-    auto& depth2color_extrinsics = _depth_to_other_extrinsics[COLOR];
-    auto color_intrinsics = _stream_intrinsics[COLOR];
+    auto infrargb_intrinsics = _stream_intrinsics[INFRARGB];
     auto depth_intrinsics = _stream_intrinsics[DEPTH];
     auto image_depth16 = reinterpret_cast<const uint16_t*>(_image[DEPTH].data);
-    unsigned char* color_data = _image[COLOR].data;
+    auto width_ratio = static_cast<float>(infrargb_intrinsics.width) / static_cast<float>(depth_intrinsics.width);
+    auto height_ratio = static_cast<float>(infrargb_intrinsics.height) / static_cast<float>(depth_intrinsics.height);
+
+    unsigned char* color_data = _image[INFRARGB].data;
     const float MIN_DEPTH_THRESHOLD = 0.f;
     const float MAX_DEPTH_THRESHOLD = 5.f;
     float depth_point[3], scaled_depth;
@@ -1240,7 +1242,6 @@ void BaseRealSenseNode::publishITRTopic(sensor_msgs::PointCloud2& msg_pointcloud
                 scaled_depth = static_cast<float>(*image_depth16) * _depth_scale_meters;
                 float depth_pixel[2] = {static_cast<float>(x), static_cast<float>(y)};
                 rs2_deproject_pixel_to_point(depth_point, &depth_intrinsics, depth_pixel, scaled_depth);
-
                 if (depth_point[2] <= MIN_DEPTH_THRESHOLD || depth_point[2] > MAX_DEPTH_THRESHOLD)
                 {
                     depth_point[0] = MIN_DEPTH_THRESHOLD;