Merge pull request IntelRealSense#12 from 130s/cp7/por_d

Cherry-pick IntelRealSense#7 (Project infra RGB)
130s · Jan 15, 2019 · 14cd52e · 14cd52e
2 parents a854483 + 08fd374
commit 14cd52e
Showing 1 changed file with 13 additions and 29 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;
@@ -1252,30 +1253,13 @@ void BaseRealSenseNode::publishITRTopic(sensor_msgs::PointCloud2& msg_pointcloud
                 *iter_y = depth_point[1];
                 *iter_z = depth_point[2];
 
-                float color_point[3], color_pixel[2];
-
-                rs2_transform_point_to_point(color_point, &depth2color_extrinsics, depth_point);
-                rs2_project_point_to_pixel(color_pixel, &color_intrinsics, color_point);
-
-                if (color_pixel[1] < MIN_DEPTH_THRESHOLD || color_pixel[1] > color_intrinsics.height
-                    || color_pixel[0] < MIN_DEPTH_THRESHOLD || color_pixel[0] > color_intrinsics.width)
-                {
-                    // For out of bounds color data, default to a shade of blue in order to visually distinguish holes.
-                    // This color value is same as the librealsense out of bounds color value.
-                    *iter_r = static_cast<uint8_t>(96);
-                    *iter_g = static_cast<uint8_t>(157);
-                    *iter_b = static_cast<uint8_t>(198);
-                }
-                else
-                {
-                    auto i = static_cast<int>(color_pixel[0]);
-                    auto j = static_cast<int>(color_pixel[1]);
+                auto i = static_cast<int>(depth_pixel[0] * 2);
+                auto j = static_cast<int>(depth_pixel[1] * 2);
 
-                    auto offset = i * 3 + j * color_intrinsics.width * 3;
-                    *iter_r = static_cast<uint8_t>(color_data[offset]);
-                    *iter_g = static_cast<uint8_t>(color_data[offset + 1]);
-                    *iter_b = static_cast<uint8_t>(color_data[offset + 2]);
-                }
+                auto offset = i * 3 + j * infrargb_intrinsics.width * 3;
+                *iter_r = static_cast<uint8_t>(color_data[offset]);
+                *iter_g = static_cast<uint8_t>(color_data[offset + 1]);
+                *iter_b = static_cast<uint8_t>(color_data[offset + 2]);
 
                 ++image_depth16;
                 ++iter_x; ++iter_y; ++iter_z;