Distortion normalization improvement + fix "folding" on large distort…

…ions (#3009)

This fixes distortion behavior when the SDFormat
<ignition:legacy_mode> parameter is set to false in
the distortion context. The fix ensures that pixel
coordinates projected to camera plane for distortion
and disables cropping.

This also fixes a wraparound issue for extreme distortions.

Signed-off-by: Audrow Nash <audrow@hey.com>

Co-authored-by: kbjeppes <kaden.b.jeppesen@nasa.gov>
Co-authored-by: Audrow Nash <audrow@hey.com>

gazebo/rendering/Distortion.cc

@@ -60,6 +60,14 @@ namespace gazebo
       /// black pixels at the corners of the image.
       public: bool distortionCrop = true;
 
+      /// \brief Use the legacy distortion mode. If this is set to false, the
+      /// new mode modifies how Brown's distortion equations are applied to
+      /// better reflect real distortion. The image is projected from image
+      /// plane to camera plane to apply distortion equations, then projected
+      /// back to image plane. Note that the new distortion doesn't allow
+      /// the image to be cropped.
+      public: bool legacyMode = true;
+
       /// \brief Lens distortion compositor
       public: Ogre::CompositorInstance *lensDistortionInstance;
 
@@ -122,6 +130,16 @@ void Distortion::Load(sdf::ElementPtr _sdf)
   {
     this->dataPtr->compositorName = _sdf->Get<std::string>(compositorName);
   }
+  const std::string legacyMode = "ignition:legacy_mode";
+  if (_sdf->HasElement(legacyMode))
+  {
+    this->dataPtr->legacyMode = _sdf->Get<bool>(legacyMode);
+  }
+  if (!this->dataPtr->legacyMode && this->dataPtr->distortionCrop)
+  {
+    gzwarn << "Enable legacy mode to use distortion cropping" << std::endl;
+    this->dataPtr->distortionCrop = false;
+  }
 }
 
 //////////////////////////////////////////////////
@@ -160,12 +178,16 @@ void Distortion::SetCamera(CameraPtr _camera)
   // seems to work best with a square distortion map texture
   unsigned int texSide = _camera->ImageHeight() > _camera->ImageWidth() ?
       _camera->ImageHeight() : _camera->ImageWidth();
+  // calculate focal length from largest fov
+  const double fov = _camera->ImageHeight() > _camera->ImageWidth() ?
+      _camera->VFOV().Radian() : _camera->HFOV().Radian();
+  const double focalLength = texSide/(2*tan(fov/2));
   this->dataPtr->distortionTexWidth = texSide - 1;
   this->dataPtr->distortionTexHeight = texSide - 1;
   unsigned int imageSize =
       this->dataPtr->distortionTexWidth * this->dataPtr->distortionTexHeight;
-  double incrU = 1.0 / this->dataPtr->distortionTexWidth;
-  double incrV = 1.0 / this->dataPtr->distortionTexHeight;
+  double colStepSize = 1.0 / this->dataPtr->distortionTexWidth;
+  double rowStepSize = 1.0 / this->dataPtr->distortionTexHeight;
 
   // initialize distortion map
   this->dataPtr->distortionMap.resize(imageSize);
@@ -174,30 +196,96 @@ void Distortion::SetCamera(CameraPtr _camera)
     this->dataPtr->distortionMap[i] = -1;
   }
 
+  ignition::math::Vector2d distortionCenterCoordinates(
+      this->dataPtr->lensCenter.X() * this->dataPtr->distortionTexWidth,
+      this->dataPtr->lensCenter.Y() * this->dataPtr->distortionTexWidth);
+
+  // declare variables before the loop
+  const auto unsetPixelVector =  ignition::math::Vector2d(-1, -1);
+  ignition::math::Vector2d normalizedLocation,
+      distortedLocation,
+      newDistortedCoordinates,
+      currDistortedCoordinates;
+  unsigned int distortedIdx,
+      distortedCol,
+      distortedRow;
+  double normalizedColLocation, normalizedRowLocation;
+
   // fill the distortion map
-  for (unsigned int i = 0; i < this->dataPtr->distortionTexHeight; ++i)
+  for (unsigned int mapRow = 0; mapRow < this->dataPtr->distortionTexHeight;
+    ++mapRow)
   {
-    double v = i*incrV;
-    for (unsigned int j = 0; j < this->dataPtr->distortionTexWidth; ++j)
+    normalizedRowLocation = mapRow*rowStepSize;
+    for (unsigned int mapCol = 0; mapCol < this->dataPtr->distortionTexWidth;
+      ++mapCol)
     {
-      double u = j*incrU;
-      ignition::math::Vector2d uv(u, v);
-      ignition::math::Vector2d out =
-        this->Distort(uv, this->dataPtr->lensCenter,
+      normalizedColLocation = mapCol*colStepSize;
+
+      normalizedLocation[0] = normalizedColLocation;
+      normalizedLocation[1] = normalizedRowLocation;
+
+      if (this->dataPtr->legacyMode)
+      {
+        distortedLocation = this->Distort(
+            normalizedLocation,
+            this->dataPtr->lensCenter,
             this->dataPtr->k1, this->dataPtr->k2, this->dataPtr->k3,
             this->dataPtr->p1, this->dataPtr->p2);
+      }
+      else
+      {
+        distortedLocation = this->Distort(
+            normalizedLocation,
+            this->dataPtr->lensCenter,
+            this->dataPtr->k1, this->dataPtr->k2, this->dataPtr->k3,
+            this->dataPtr->p1, this->dataPtr->p2,
+            this->dataPtr->distortionTexWidth,
+            focalLength);
+      }
 
       // compute the index in the distortion map
-      unsigned int idxU = out.X() * this->dataPtr->distortionTexWidth;
-      unsigned int idxV = out.Y() * this->dataPtr->distortionTexHeight;
-
-      if (idxU < this->dataPtr->distortionTexWidth &&
-          idxV < this->dataPtr->distortionTexHeight)
+      distortedCol = distortedLocation.X() * this->dataPtr->distortionTexWidth;
+      distortedRow = distortedLocation.Y() *
+        this->dataPtr->distortionTexHeight;
+
+      // Note that the following makes sure that, for significant distortions,
+      // there is not a problem where the distorted image seems to fold over
+      // itself. This is accomplished by favoring pixels closer to the center
+      // of distortion, and this change applies to both the legacy and
+      // nonlegacy distortion modes.
+
+      // Make sure the distorted pixel is within the texture dimensions
+      if (distortedCol < this->dataPtr->distortionTexWidth &&
+          distortedRow < this->dataPtr->distortionTexHeight)
       {
-        unsigned int mapIdx = idxV * this->dataPtr->distortionTexWidth + idxU;
-        this->dataPtr->distortionMap[mapIdx] = uv;
+        distortedIdx = distortedRow * this->dataPtr->distortionTexWidth +
+          distortedCol;
+
+        // check if the index has already been set
+        if (this->dataPtr->distortionMap[distortedIdx] != unsetPixelVector)
+        {
+          // grab current coordinates that map to this destination
+          currDistortedCoordinates =
+            this->dataPtr->distortionMap[distortedIdx] *
+            this->dataPtr->distortionTexWidth;
+
+          // grab new coordinates to map to
+          newDistortedCoordinates[0] = mapCol;
+          newDistortedCoordinates[1] = mapRow;
+
+          // use the new mapping if it is closer to the center of the distortion
+          if (newDistortedCoordinates.Distance(distortionCenterCoordinates) <
+              currDistortedCoordinates.Distance(distortionCenterCoordinates))
+          {
+            this->dataPtr->distortionMap[distortedIdx] = normalizedLocation;
+          }
+        }
+        else
+        {
+          this->dataPtr->distortionMap[distortedIdx] = normalizedLocation;
+        }
       }
-      // else: pixel maps outside the image bounds.
+      // else: mapping is outside of the image bounds.
       // This is expected and normal to ensure
       // no black borders; carry on
     }
@@ -395,11 +483,20 @@ ignition::math::Vector2d Distortion::Distort(
     const ignition::math::Vector2d &_in,
     const ignition::math::Vector2d &_center, double _k1, double _k2, double _k3,
     double _p1, double _p2)
+{
+  return Distort(_in, _center, _k1, _k2, _k3, _p1, _p2, 1u, 1.0);
+}
+
+//////////////////////////////////////////////////
+ignition::math::Vector2d Distortion::Distort(
+    const ignition::math::Vector2d &_in,
+    const ignition::math::Vector2d &_center, double _k1, double _k2, double _k3,
+    double _p1, double _p2, unsigned int _width, double _f)
 {
   // apply Brown's distortion model, see
   // http://en.wikipedia.org/wiki/Distortion_%28optics%29#Software_correction
 
-  ignition::math::Vector2d normalized2d = _in - _center;
+  ignition::math::Vector2d normalized2d = (_in - _center)*(_width/_f);
   ignition::math::Vector3d normalized(normalized2d.X(), normalized2d.Y(), 0);
   double rSq = normalized.X() * normalized.X() +
                normalized.Y() * normalized.Y();
@@ -416,14 +513,15 @@ ignition::math::Vector2d Distortion::Distort(
   dist.Y() += _p1 * (rSq + 2 * (normalized.Y()*normalized.Y())) +
       2 * _p2 * normalized.X() * normalized.Y();
 
-  return _center + ignition::math::Vector2d(dist.X(), dist.Y());
+  return ((_center*_width) +
+    ignition::math::Vector2d(dist.X(), dist.Y())*_f)/_width;
 }
 
 //////////////////////////////////////////////////
 void Distortion::SetCrop(const bool _crop)
 {
   // Only update the distortion scale if the crop value is going to flip.
-  if (this->dataPtr->distortionCrop != _crop)
+  if (this->dataPtr->distortionCrop != _crop && this->dataPtr->legacyMode)
   {
     this->dataPtr->distortionCrop = _crop;
     this->CalculateAndApplyDistortionScale();

gazebo/rendering/Distortion.hh

@@ -54,7 +54,8 @@ namespace gazebo
       public: void SetCamera(CameraPtr _camera);
 
       /// \brief Set whether to crop the black border around the distorted
-      /// image points.
+      /// image points. Note that cropping only occurs for the legacy mode
+      /// distortion implementation.
       /// \param[in] _crop True to crop the black border
       /// \sa Crop
       public: void SetCrop(const bool _crop);
@@ -104,6 +105,24 @@ namespace gazebo
                   double _k1, double _k2, double _k3,
                   double _p1, double _p2);
 
+      /// \brief Apply distortion model using camera coordinates projection
+      /// \param[in] _in Input uv coordinate.
+      /// \param[in] _center Normalized distortion center.
+      /// \param[in] _k1 Radial distortion coefficient k1.
+      /// \param[in] _k2 Radial distortion coefficient k2.
+      /// \param[in] _k3 Radial distortion coefficient k3.
+      /// \param[in] _p1 Tangential distortion coefficient p1.
+      /// \param[in] _p2 Tangential distortion coefficient p2.
+      /// \param[in] _width Width of the image texture in pixels.
+      /// \param[in] _f Focal length in pixels.
+      /// \return Distorted coordinate.
+      public: static ignition::math::Vector2d Distort(
+                  const ignition::math::Vector2d &_in,
+                  const ignition::math::Vector2d &_center,
+                  double _k1, double _k2, double _k3,
+                  double _p1, double _p2,
+                  unsigned int _width, double _f);
+
       /// \brief get the distortion map value.
       /// \return the distortion map value at the specified index,
       /// or (-1, -1) if the index

gazebo/test/ServerFixture.cc

@@ -578,7 +578,9 @@ void ServerFixture::SpawnCamera(const std::string &_modelName,
     const std::string &_noiseType, double _noiseMean, double _noiseStdDev,
     bool _distortion, double _distortionK1, double _distortionK2,
     double _distortionK3, double _distortionP1, double _distortionP2,
-    double _cx, double _cy)
+    double _cx, double _cy,
+    bool _legacyMode,
+    double _horizontalFov)
 {
   msgs::Factory msg;
   std::ostringstream newModelStr;
@@ -594,7 +596,7 @@ void ServerFixture::SpawnCamera(const std::string &_modelName,
     << "    <update_rate>" << _rate << "</update_rate>"
     << "    <visualize>true</visualize>"
     << "    <camera>"
-    << "      <horizontal_fov>0.78539816339744828</horizontal_fov>"
+    << "      <horizontal_fov>" << _horizontalFov << "</horizontal_fov>"
     << "      <image>"
     << "        <width>" << _width << "</width>"
     << "        <height>" << _height << "</height>"
@@ -623,6 +625,9 @@ void ServerFixture::SpawnCamera(const std::string &_modelName,
     << "        <p1>" << _distortionP1 << "</p1>"
     << "        <p2>" << _distortionP2 << "</p2>"
     << "        <center>" << _cx << " " << _cy << "</center>"
+    << "        <ignition:legacy_mode>"
+      << (_legacyMode ? "true" : "false")
+      << "</ignition:legacy_mode>"
     << "      </distortion>";
   }
 

gazebo/test/ServerFixture.hh

@@ -254,6 +254,8 @@ namespace gazebo
     /// \param[in] _distortionP2 Distortion coefficient p2.
     /// \param[in] _cx Normalized optical center x, used for distortion.
     /// \param[in] _cy Normalized optical center y, used for distortion.
+    /// \param[in] _legacyMode Use the legacy distortion model.
+    /// \param[in] _horizontalFov The horizontal field of view.
     protected: void SpawnCamera(const std::string &_modelName,
                    const std::string &_cameraName,
                    const ignition::math::Vector3d &_pos =
@@ -267,7 +269,9 @@ namespace gazebo
                    bool _distortion = false, double _distortionK1 = 0.0,
                    double _distortionK2 = 0.0, double _distortionK3 = 0.0,
                    double _distortionP1 = 0.0, double _distortionP2 = 0.0,
-                   double _cx = 0.5, double _cy = 0.5);
+                   double _cx = 0.5, double _cy = 0.5,
+                   bool _legacyMode = true,
+                   double _horizontalFov = 0.78539816339744828);
 
     /// \brief Spawn a wide angle camera.
     /// \param[in] _modelName Name of the model.