Bugfix on the color channel of point clouds from DepthCamera

gazebo/rendering/DepthCamera.hh

@@ -104,6 +104,14 @@ namespace gazebo
           unsigned int, const std::string &)>  _subscriber);
 
       /// \brief Connect a to the new rgb point cloud signal
+      /// Point coordinates and color are stored in a vector4f.
+      /// The first three channels are for the XYZ coordinates.
+      /// RGB is packed into the 4th channel as a float.
+      /// Unpacking can be performed as follows:
+      ///   uint8_t red = floor(_pcd[4 * index + 3] / 256.0f / 256.0f);
+      ///   uint8_t green = floor((_pcd[4 * index + 3] - red * 256.0f * 256.0f) / 256.0f);
+      ///   uint8_t blue = floor(_pcd[4 * index + 3] - red * 256.0f * 256.0f - green * 256.0f);
+      /// red, green and blue are within the range [0, 255].
       /// \param[in] _subscriber Subscriber callback function
       /// \return Pointer to the new Connection. This must be kept in scope
       public: event::ConnectionPtr ConnectNewRGBPointCloud(

media/materials/programs/depth_points_map.frag

@@ -9,9 +9,7 @@ varying vec4 point;
 
 void main()
 {
-  //vec3 color = 255 * texture2D(tex, vec2(gl_FragCoord.s / width , gl_FragCoord.t / height)).xyz;
-  vec3 color = vec3(80, 0, 0);
-  // int rgb = int(color.r) << 16 | int(color.g) << 8 | int(color.b);
-  int rgb = 1;
+  vec3 color = 255.0f * texture2D(tex, vec2(gl_FragCoord.s / width , gl_FragCoord.t / height)).xyz;
+  float rgb = color.b + color.g * 256.0f + color.r * 256.0f * 256.0f;
   gl_FragColor = vec4(point.x, -point.y, -point.z, rgb);
 }

test/integration/depth_camera.cc

@@ -27,15 +27,34 @@ class DepthCameraSensorTest : public ServerFixture
   /// \brief Test placing a box in front of a depth camera
   public: void DepthUnitBox();
 
+  /// \brief Verify point cloud color generated by depth camera
+  public: void PointCloudColor();
+
   /// \brief Depth camera image callback
   /// \param[in] _msg Message with image data containing raw depth values.
   public: void OnImage(ConstImageStampedPtr &_msg);
 
+  /// \brief Depth camera image callback
+  /// \param[in] _pc Point cloud data
+  /// \param[in] _width Point cloud image width
+  /// \param[in] _height Point cloud image height
+  /// \param[in] depth Point cloud image depth
+  /// \param[in] format Point cloud image format
+  public: void OnNewPointCloud(const float * _pc,
+      unsigned int _width, unsigned int _height, unsigned int _depth,
+      const std::string & _format);
+
   /// \brief Counter for the numbder of image messages received.
-  public: unsigned int depthCount;
+  public: unsigned int depthCount = 0u;
 
   /// \brief Depth data buffer.
-  public: float *depthBuffer;
+  public: float *depthBuffer = nullptr;
+
+  /// \brief Counter for the numbder of point cloud data received.
+  public: unsigned int pcCount = 0u;
+
+  /// \brief Point cloud buffer
+  public: float *pcBuffer = nullptr;
 };
 
 /////////////////////////////////////////////////
@@ -187,6 +206,102 @@ TEST_F(DepthCameraSensorTest, DepthUnitBox)
   DepthUnitBox();
 }
 
+/////////////////////////////////////////////////
+void DepthCameraSensorTest::OnNewPointCloud(const float * _pc,
+    unsigned int _width, unsigned int _height, unsigned int _depth,
+    const std::string &_format)
+{
+  float f;
+  unsigned int pcSamples = _width * _height * 4;
+  unsigned int pcBufferSize = pcSamples * sizeof(f);
+  if (!this->pcBuffer)
+    this->pcBuffer = new float[pcSamples];
+  memcpy(this->pcBuffer, _pc, pcBufferSize);
+  this->pcCount++;
+}
+
+/////////////////////////////////////////////////
+void DepthCameraSensorTest::PointCloudColor()
+{
+  Load("worlds/depth_camera2.world");
+  sensors::SensorManager *mgr = sensors::SensorManager::Instance();
+
+  // Create the camera sensor
+  std::string sensorName = "default::camera_model::my_link::camera";
+
+  // Get a pointer to the depth camera sensor
+  sensors::DepthCameraSensorPtr sensor =
+     std::dynamic_pointer_cast<sensors::DepthCameraSensor>
+     (mgr->GetSensor(sensorName));
+
+  // Make sure the above dynamic cast worked.
+  ASSERT_NE(nullptr, sensor);
+
+  EXPECT_EQ(sensor->ImageWidth(), 640u);
+  EXPECT_EQ(sensor->ImageHeight(), 480u);
+  EXPECT_TRUE(sensor->IsActive());
+
+  rendering::DepthCameraPtr depthCamera = sensor->DepthCamera();
+  ASSERT_NE(nullptr, depthCamera);
+
+  event::ConnectionPtr c = depthCamera->ConnectNewRGBPointCloud(
+      std::bind(&DepthCameraSensorTest::OnNewPointCloud, this,
+      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
+      std::placeholders::_4, std::placeholders::_5));
+
+  unsigned int framesToWait = 10;
+  // wait for a few depth image callbacks
+  int i = 0;
+  while (i < 300 && this->pcCount < framesToWait)
+  {
+    common::Time::MSleep(20);
+    i++;
+  }
+  EXPECT_GE(this->pcCount, framesToWait);
+
+  float boxDist = 2.5;
+  // verify point cloud color
+  for (unsigned int i = 0; i < sensor->ImageWidth(); i++)
+  {
+    for (unsigned int j = 0; j < sensor->ImageHeight(); j++)
+    {
+      unsigned int index = (j * sensor->ImageWidth()) + i;
+      float z = this->pcBuffer[4 * index + 2];
+      float color = this->pcBuffer[4 * index + 3];
+
+      unsigned int r = floor(color / 256.0 / 256.0);
+      unsigned int g = floor((color - r * 256.0 * 256.0) / 256.0);
+      unsigned int b = floor(color - r * 256.0 * 256.0 - g * 256.0);
+
+      if (ignition::math::equal(z, boxDist, 1e-2f))
+      {
+        EXPECT_GT(g, 0.0);
+        EXPECT_GT(g, r);
+        EXPECT_GT(g, b);
+      }
+      else
+      {
+        EXPECT_FLOAT_EQ(r, 0.0);
+        EXPECT_FLOAT_EQ(g, 0.0);
+        // todo(anyone) blue pixel component always returns 1 instead of 0.
+        EXPECT_NEAR(b, 0.0, 1.0);
+      }
+      // igndbg << "(" << r << " " << g << " " << b << ", z: " << z << ")";
+    }
+    // igndbg << std::endl;
+  }
+
+  depthCamera.reset();
+}
+
+/////////////////////////////////////////////////
+/// \brief Test depth camera sensor point cloud color values
+TEST_F(DepthCameraSensorTest, PointCloudColor)
+{
+  PointCloudColor();
+}
+
+
 int main(int argc, char **argv)
 {
   ::testing::InitGoogleTest(&argc, argv);