Fix lidar 1st pass texture resolution calculation for FOV < 90 degrees (

#1012)

Signed-off-by: Ian Chen <[email protected]>

ogre2/src/Ogre2GpuRays.cc

@@ -761,10 +761,13 @@ void Ogre2GpuRays::ConfigureCamera()
   // Configure first pass texture size
   // Each cubemap texture covers 90 deg FOV so determine number of samples
   // within the view for both horizontal and vertical FOV
-  unsigned int hs = static_cast<unsigned int>(
-      GZ_PI * 0.5 / hfovAngle.Radian() * this->RangeCount());
-  unsigned int vs = static_cast<unsigned int>(
-      GZ_PI * 0.5 / vfovAngle * this->VerticalRangeCount());
+  unsigned int hs = (hfovAngle.Radian() < GZ_PI_2) ? this->RangeCount() :
+    static_cast<unsigned int>(
+    GZ_PI_2 / hfovAngle.Radian() * this->RangeCount());
+
+  unsigned int vs = (vfovAngle < GZ_PI_2) ? this->VerticalRangeCount() :
+    static_cast<unsigned int>(
+    GZ_PI_2 / vfovAngle * this->VerticalRangeCount());
 
   // get the max number from the two
   unsigned int v = std::max(hs, vs);
@@ -778,12 +781,12 @@ void Ogre2GpuRays::ConfigureCamera()
   v |= v >> 16;
   v++;
 
-  // limit min texture size to 128
   // This is needed for large fov with low sample count,
   // e.g. 360 degrees and only 4 samples. Otherwise the depth data returned are
   // inaccurate.
   // \todo(anyone) For small fov, we shouldn't need such a high min texture size
-  // requirement, e.g. a single ray lidar only needs 1x1 texture. Look for ways
+  // requirement, e.g. a single ray lidar only needs 1x1 texture. However,
+  // using lower res textures also give inaccurate results. Look for ways
   // to compute the optimal min texture size
   unsigned int min1stPassSamples = 128u;
 
@@ -982,6 +985,7 @@ void Ogre2GpuRays::Setup1stPass()
     Ogre::TextureFlags::RenderToTexture, Ogre::TextureTypes::Type2D);
   this->dataPtr->colorTexture->setResolution(this->dataPtr->w1st,
                                              this->dataPtr->h1st);
+  this->dataPtr->colorTexture->setNumMipmaps(1u);
   this->dataPtr->colorTexture->setPixelFormat(Ogre::PFG_R16_UNORM);
   this->dataPtr->colorTexture->scheduleTransitionTo(
     Ogre::GpuResidency::Resident);
@@ -992,6 +996,7 @@ void Ogre2GpuRays::Setup1stPass()
     Ogre::TextureFlags::RenderToTexture, Ogre::TextureTypes::Type2D);
   this->dataPtr->depthTexture->setResolution(this->dataPtr->w1st,
                                              this->dataPtr->h1st);
+  this->dataPtr->depthTexture->setNumMipmaps(1u);
   this->dataPtr->depthTexture->setPixelFormat(Ogre::PFG_D32_FLOAT);
   this->dataPtr->depthTexture->scheduleTransitionTo(
     Ogre::GpuResidency::Resident);
@@ -1002,6 +1007,7 @@ void Ogre2GpuRays::Setup1stPass()
     Ogre::TextureFlags::RenderToTexture, Ogre::TextureTypes::Type2D);
   this->dataPtr->particleTexture->setResolution(this->dataPtr->w1st / 2u,
                                              this->dataPtr->h1st/ 2u);
+  this->dataPtr->particleTexture->setNumMipmaps(1u);
   this->dataPtr->particleTexture->setPixelFormat(Ogre::PFG_RGBA8_UNORM);
   this->dataPtr->particleTexture->scheduleTransitionTo(
     Ogre::GpuResidency::Resident);
@@ -1012,6 +1018,7 @@ void Ogre2GpuRays::Setup1stPass()
     Ogre::TextureFlags::RenderToTexture, Ogre::TextureTypes::Type2D);
   this->dataPtr->particleDepthTexture->setResolution(this->dataPtr->w1st / 2u,
                                                      this->dataPtr->h1st / 2u);
+  this->dataPtr->particleDepthTexture->setNumMipmaps(1u);
   this->dataPtr->particleDepthTexture->setPixelFormat(Ogre::PFG_D32_FLOAT);
   this->dataPtr->particleDepthTexture->scheduleTransitionTo(
     Ogre::GpuResidency::Resident);

test/integration/gpu_rays.cc

@@ -697,8 +697,8 @@ TEST_F(GpuRaysTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SingleRay))
     GTEST_SKIP() << "Unsupported on apple, see issue #35.";
   #endif
 
-  // Test GPU single ray box intersection.
-  // Place GPU above box looking downwards
+  // Test single ray box intersection.
+  // Place ray above box looking downwards
   // ray should intersect with center of box
 
   const double hMinAngle = 0.0;
@@ -713,7 +713,7 @@ TEST_F(GpuRaysTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SingleRay))
 
   VisualPtr root = scene->RootVisual();
 
-  // Create first ray caster
+  // Create ray caster
   gz::math::Pose3d testPose(gz::math::Vector3d(0, 0, 7),
       gz::math::Quaterniond(0, GZ_PI/2.0, 0));
 
@@ -731,7 +731,7 @@ TEST_F(GpuRaysTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SingleRay))
 
   // box in the center
   gz::math::Pose3d box01Pose(gz::math::Vector3d(0, 0, 4.5),
-                                   gz::math::Quaterniond::Identity);
+                             gz::math::Quaterniond::Identity);
   VisualPtr visualBox1 = scene->CreateVisual("UnitBox1");
   visualBox1->AddGeometry(scene->CreateBox());
   visualBox1->SetWorldPosition(box01Pose.Pos());
@@ -754,9 +754,17 @@ TEST_F(GpuRaysTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SingleRay))
   int mid = 0;
   double unitBoxSize = 1.0;
   double expectedRangeAtMidPointBox = testPose.Pos().Z() -
-      (abs(box01Pose.Pos().Z()) + unitBoxSize/2);
+      (std::abs(box01Pose.Pos().Z()) + unitBoxSize / 2);
 
-  // rays caster 1 should see box01 and box02
+  // ray should detect box
+  EXPECT_NEAR(scan[mid], expectedRangeAtMidPointBox, LASER_TOL);
+
+  gz::math::Pose3d newBox01Pose(gz::math::Vector3d(0, 0, 3.5),
+                                gz::math::Quaterniond::Identity);
+  visualBox1->SetWorldPosition(newBox01Pose.Pos());
+  gpuRays->Update();
+  expectedRangeAtMidPointBox = testPose.Pos().Z() -
+      (std::abs(newBox01Pose.Pos().Z()) + unitBoxSize / 2);
   EXPECT_NEAR(scan[mid], expectedRangeAtMidPointBox, LASER_TOL);
 
   c.reset();