Specify wide angle camera cube map texture format (#2960)

The WideAngleCamera has a texture for a cube map that uses the same pixel format as the camera image. If there are intermediate shaders applied to the camera, the choice of this image format may cause pixel values to be truncated. To provide flexibility to the user, the pixel format for the cube map texture can be specified with a custom SDFormat element in `//camera/lens/ignition:env_texture_format` using the same values as `//camera/image/format`. Add friend WideAngleCamera statement to Camera.hh to allow access to private OgrePixelFormat method. Camera: recognize R_FLOAT16 and R_FLOAT32 to match ignition. * PhysicsEngine_TEST: speed up test Test update_rate parameter value of 3.03 instead of 0.03 to reduce test time from over 30 seconds to about 1 second. Signed-off-by: Steve Peters <scpeters@openrobotics.org> * Add test for wide angle camera with texture plugin * Add check that grayscale image isn't black Signed-off-by: Audrow Nash <audrow@hey.com> Co-authored-by: Steve Peters <scpeters@openrobotics.org>
gazebosim · Apr 14, 2021 · 32c6869 · 32c6869
1 parent 61265a9
commit 32c6869
Show file tree

Hide file tree

Showing 8 changed files with 277 additions and 8 deletions.
diff --git a/gazebo/physics/PhysicsEngine_TEST.cc b/gazebo/physics/PhysicsEngine_TEST.cc
@@ -125,7 +125,7 @@ void PhysicsEngineTest::PhysicsEngineParam(const std::string &_physicsEngine)
     {
       boost::any value;
       double maxStepSize = 0.02;
-      double realTimeUpdateRate = 0.03;
+      double realTimeUpdateRate = 3.03;
       double realTimeFactor = 0.04;
       ignition::math::Vector3d gravity(0, 0, 0);
       ignition::math::Vector3d magField(0.1, 0.1, 0.1);

diff --git a/gazebo/rendering/Camera.cc b/gazebo/rendering/Camera.cc
@@ -909,9 +909,9 @@ unsigned int Camera::ImageDepth() const
   else if ((imgFmt == "BAYER_RGGB8") || (imgFmt == "BAYER_BGGR8") ||
             (imgFmt == "BAYER_GBRG8") || (imgFmt == "BAYER_GRBG8"))
     return 1;
-  else if (imgFmt == "FLOAT32")
+  else if (imgFmt == "FLOAT32" || imgFmt == "R_FLOAT32")
     return 4;
-  else if (imgFmt == "FLOAT16")
+  else if (imgFmt == "FLOAT16" || imgFmt == "R_FLOAT16")
     return 2;
   else
   {
@@ -980,9 +980,9 @@ int Camera::OgrePixelFormat(const std::string &_format)
     result = static_cast<int>(Ogre::PF_BYTE_RGB);
   else if (_format == "B8G8R8" || _format == "BGR_INT8")
     result = static_cast<int>(Ogre::PF_BYTE_BGR);
-  else if (_format == "FLOAT32")
+  else if (_format == "FLOAT32" || _format == "R_FLOAT32")
     result = static_cast<int>(Ogre::PF_FLOAT32_R);
-  else if (_format == "FLOAT16")
+  else if (_format == "FLOAT16" || _format == "R_FLOAT16")
     result = static_cast<int>(Ogre::PF_FLOAT16_R);
   else if (_format == "R16G16B16" || _format == "RGB_INT16"
       || _format == "RGB_UINT16")

diff --git a/gazebo/rendering/Camera.hh b/gazebo/rendering/Camera.hh
@@ -849,6 +849,11 @@ namespace gazebo
       /// \return Integer representation of the Ogre image format
       private: static int OgrePixelFormat(const std::string &_format);
 
+      /// \brief Allow WideAngleCamera::Load to call OgrePixelFormat.
+      /// To avoid needing to include WideAngleCamera.hh, just befriend
+      /// the entire class.
+      friend class WideAngleCamera;
+
       /// \brief Receive command message.
       /// \param[in] _msg Camera Command message.
       private: void OnCmdMsg(ConstCameraCmdPtr &_msg);

diff --git a/gazebo/rendering/Camera_TEST.cc b/gazebo/rendering/Camera_TEST.cc
@@ -191,7 +191,7 @@ TEST_F(Camera_TEST, Create)
 
     unsigned int width = 500;
     unsigned int height = 300;
-    std::string format = "FLOAT16";
+    std::string format = "R_FLOAT16";
     double hfov = 1.05;
     double near = 0.001;
     double far = 200.0;
@@ -239,7 +239,7 @@ TEST_F(Camera_TEST, Create)
 
     unsigned int width = 500;
     unsigned int height = 300;
-    std::string format = "FLOAT32";
+    std::string format = "R_FLOAT32";
     double hfov = 1.05;
     double near = 0.001;
     double far = 200.0;

diff --git a/gazebo/rendering/WideAngleCamera.cc b/gazebo/rendering/WideAngleCamera.cc
@@ -457,6 +457,10 @@ void WideAngleCamera::Load()
 {
   Camera::Load();
 
+  // Cube map texture format defaults to matching image pixel format
+  this->dataPtr->envCubeMapTextureFormat =
+    static_cast<Ogre::PixelFormat>(this->imageFormat);
+
   this->CreateEnvCameras();
 
   if (this->sdf->HasElement("lens"))
@@ -467,6 +471,13 @@ void WideAngleCamera::Load()
 
     if (sdfLens->HasElement("env_texture_size"))
       this->dataPtr->envTextureSize = sdfLens->Get<int>("env_texture_size");
+
+    const std::string envTextureFormat = "ignition:env_texture_format";
+    if (sdfLens->HasElement(envTextureFormat))
+    {
+      this->dataPtr->envCubeMapTextureFormat = static_cast<Ogre::PixelFormat>(
+        this->OgrePixelFormat(sdfLens->Get<std::string>(envTextureFormat)));
+    }
   }
   else
     this->dataPtr->lens->Load();
@@ -660,7 +671,7 @@ void WideAngleCamera::CreateEnvRenderTexture(const std::string &_textureName)
           this->dataPtr->envTextureSize,
           this->dataPtr->envTextureSize,
           0,
-          static_cast<Ogre::PixelFormat>(this->imageFormat),
+          this->dataPtr->envCubeMapTextureFormat,
           Ogre::TU_RENDERTARGET,
           0,
           false,

diff --git a/gazebo/rendering/WideAngleCameraPrivate.hh b/gazebo/rendering/WideAngleCameraPrivate.hh
@@ -20,6 +20,7 @@
 #include <mutex>
 
 #include "gazebo/msgs/msgs.hh"
+#include "gazebo/rendering/ogre_gazebo.h"
 #include "gazebo/util/system.hh"
 
 
@@ -59,6 +60,9 @@ namespace gazebo
       /// \brief Viewports for the render targets
       public: Ogre::Viewport *envViewports[6];
 
+      /// \brief Pixel format for cube map texture
+      public: Ogre::PixelFormat envCubeMapTextureFormat;
+
       /// \brief A single cube map texture
       public: Ogre::Texture *envCubeMapTexture;
 

diff --git a/test/integration/wideanglecamera_sensor.cc b/test/integration/wideanglecamera_sensor.cc
@@ -225,3 +225,112 @@ TEST_F(WideAngleCameraSensor, Projection)
   EXPECT_LT(screenPt.Z(), 1.0);
 #endif
 }
+
+/////////////////////////////////////////////////
+TEST_F(WideAngleCameraSensor, TextureFormat)
+{
+#if not defined(__APPLE__)
+  Load("worlds/test/issue_2928_wide_angle_camera_texture_format.world");
+
+  // Make sure the render engine is available.
+  if (rendering::RenderEngine::Instance()->GetRenderPathType() ==
+      rendering::RenderEngine::NONE)
+  {
+    gzerr << "No rendering engine, unable to run wide angle camera test\n";
+    return;
+  }
+
+  const unsigned int width  = 12;
+  const unsigned int height = 12;
+
+  {  // check camera with default texture format
+    sensors::SensorPtr sensor = sensors::get_sensor(
+      "wide_angle_camera_sensor_with_default_texture");
+    sensors::WideAngleCameraSensorPtr cameraSensor =
+        std::dynamic_pointer_cast<sensors::WideAngleCameraSensor>(sensor);
+
+    imageCount = 0;
+    img = new unsigned char[width * height * 3];
+    event::ConnectionPtr c =
+      cameraSensor->Camera()->ConnectNewImageFrame(
+          std::bind(&::OnNewCameraFrame, &imageCount, img,
+            std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
+            std::placeholders::_4, std::placeholders::_5));
+
+    // Get some images
+    int sleep = 0;
+    int maxSleep = 30;
+    while (imageCount < 10 && sleep < maxSleep)
+    {
+      common::Time::MSleep(50);
+      sleep++;
+    }
+
+    unsigned int rSum = 0;
+    unsigned int gSum = 0;
+    unsigned int bSum = 0;
+    for (unsigned int i = 0; i < height*width*3; i+=3)
+    {
+      unsigned int r = img[i];
+      unsigned int g = img[i+1];
+      unsigned int b = img[i+2];
+      rSum += r;
+      gSum += g;
+      bSum += b;
+    }
+
+    EXPECT_NE(rSum, gSum);
+    EXPECT_NE(rSum, bSum);
+    EXPECT_NE(gSum, bSum);
+
+    EXPECT_GT(rSum - bSum, 30000.0);
+    EXPECT_GT(rSum - gSum, 20000.0);
+
+    delete [] img;
+  }
+
+  {  // check camera with grayscale texture format
+    sensors::SensorPtr sensor = sensors::get_sensor(
+      "wide_angle_camera_sensor_with_grayscale_texture");
+    sensors::WideAngleCameraSensorPtr cameraSensor =
+        std::dynamic_pointer_cast<sensors::WideAngleCameraSensor>(sensor);
+
+    imageCount = 0;
+    img = new unsigned char[width * height * 3];
+    event::ConnectionPtr c =
+      cameraSensor->Camera()->ConnectNewImageFrame(
+          std::bind(&::OnNewCameraFrame, &imageCount, img,
+            std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
+            std::placeholders::_4, std::placeholders::_5));
+
+    // Get some images
+    int sleep = 0;
+    int maxSleep = 30;
+    while (imageCount < 10 && sleep < maxSleep)
+    {
+      common::Time::MSleep(50);
+      sleep++;
+    }
+
+    unsigned int rSum = 0;
+    unsigned int gSum = 0;
+    unsigned int bSum = 0;
+    for (unsigned int i = 0; i < height*width*3; i+=3)
+    {
+      unsigned int r = img[i];
+      unsigned int g = img[i+1];
+      unsigned int b = img[i+2];
+      rSum += r;
+      gSum += g;
+      bSum += b;
+    }
+
+    // For grayscale, all RGB channels should have the same value
+    EXPECT_EQ(rSum, gSum);
+    EXPECT_EQ(gSum, bSum);
+    EXPECT_GT(gSum, 10000u);
+
+    delete [] img;
+  }
+#endif
+}
diff --git a/worlds/test/issue_2928_wide_angle_camera_texture_format.world b/worlds/test/issue_2928_wide_angle_camera_texture_format.world
@@ -0,0 +1,140 @@
+<?xml version="1.0" ?>
+<sdf version='1.7'>
+  <world name='default' xmlns:ignition="http://ignitionrobotics.org/schema">
+    <light name='sun' type='directional'>
+      <cast_shadows>1</cast_shadows>
+      <pose>0 0 0 0 -0 0</pose>
+      <diffuse>1 1 1 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <direction>-1 0 -0.2</direction>
+      <attenuation>
+        <range>10</range>
+      </attenuation>
+      <spot>
+        <inner_angle>0</inner_angle>
+        <outer_angle>0</outer_angle>
+        <falloff>0</falloff>
+      </spot>
+    </light>
+    <scene>
+      <shadows>0</shadows>
+    </scene>
+    <model name='wide_angle_camera_with_default_texture'>
+      <static>1</static>
+      <pose>0.3 0 0.15 0 0 3.14159265</pose>
+      <link name='link'>
+        <visual name='visual'>
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </visual>
+        <sensor name='wide_angle_camera_sensor_with_default_texture' type='wideanglecamera'>
+          <camera>
+            <horizontal_fov>1.047</horizontal_fov>
+            <image>
+              <width>12</width>
+              <height>12</height>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+            <lens>
+              <type>gnomonical</type>
+              <scale_to_hfov>1</scale_to_hfov>
+              <cutoff_angle>1.5707</cutoff_angle>
+              <env_texture_size>512</env_texture_size>
+            </lens>
+          </camera>
+          <always_on>1</always_on>
+          <update_rate>10</update_rate>
+          <visualize>1</visualize>
+        </sensor>
+      </link>
+    </model>
+    <model name='wide_angle_camera_with_grayscale_texture'>
+      <static>1</static>
+      <pose>0.3 2 0.15 0 0 3.14159265</pose>
+      <link name='link'>
+        <visual name='visual'>
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </visual>
+        <sensor name='wide_angle_camera_sensor_with_grayscale_texture' type='wideanglecamera'>
+          <camera>
+            <horizontal_fov>1.047</horizontal_fov>
+            <image>
+              <width>12</width>
+              <height>12</height>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+            <lens>
+              <type>gnomonical</type>
+              <scale_to_hfov>1</scale_to_hfov>
+              <cutoff_angle>1.5707</cutoff_angle>
+              <env_texture_size>512</env_texture_size>
+              <ignition:env_texture_format>R_FLOAT16</ignition:env_texture_format>
+            </lens>
+          </camera>
+          <always_on>1</always_on>
+          <update_rate>10</update_rate>
+          <visualize>1</visualize>
+        </sensor>
+      </link>
+    </model>
+    <model name='Construction Cone 1'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <mesh>
+              <scale>10 10 10</scale>
+              <uri>model://construction_cone/meshes/construction_cone.dae</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <mesh>
+              <scale>10 10 10</scale>
+              <uri>model://construction_cone/meshes/construction_cone.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+      </link>
+      <pose>0 0 0 0 0 0</pose>
+    </model>
+    <model name='Construction Cone 2'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <mesh>
+              <scale>10 10 10</scale>
+              <uri>model://construction_cone/meshes/construction_cone.dae</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <mesh>
+              <scale>10 10 10</scale>
+              <uri>model://construction_cone/meshes/construction_cone.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+      </link>
+      <pose>0 2 0 0 0 0</pose>
+    </model>
+  </world>
+</sdf>