diff --git a/src/CameraZoomPlugin.cc b/src/CameraZoomPlugin.cc
index b0454193..a2c79a4c 100644
--- a/src/CameraZoomPlugin.cc
+++ b/src/CameraZoomPlugin.cc
@@ -102,7 +102,6 @@ class CameraZoomPlugin::Impl
     return std::optional<sim::Entity>(parent->Data());
   }
 
-
   /// \brief World occupied by the parent model.
   public: World world{kNullEntity};
 
@@ -126,8 +125,8 @@ class CameraZoomPlugin::Impl
   /// \brief Value of the most recently received zoom command.
   public: std::atomic<double> zoomCommand{1.0};
 
-  /// \brief Current horizontal field of view (radians).
-  public: double curHfov{2.0};
+  /// \brief Reference horizontal field of view (radians).
+  public: double refHfov{2.0};
 
   /// \brief Goal horizontal field of view (radians).
   public: std::atomic<double> goalHfov{2.0};
@@ -162,33 +161,30 @@ class CameraZoomPlugin::Impl
 
   /// \brief Pointer to the rendering camera
   public: rendering::CameraPtr camera;
-};
 
-/// \brief Convert from focal length to FOV for a rectilinear lens
-/// \ref https://en.wikipedia.org/wiki/Focal_length
-/// @param sensorWidth Diagonal sensor width [meter]
-/// @param focalLength The focal length [meter]
-/// @return The field of view [rad]
-[[nodiscard]] static double FocalLengthToFov(
-  const double& sensorWidth,
-  const double& focalLength);
-
-/// \brief Convert from FOV to focal length for a rectilinear lens
-/// \ref https://en.wikipedia.org/wiki/Focal_length
-/// @param sensorWidth Diagonal sensor width [meter]
-/// @param fov The field of view [rad]
-/// @return The focal length [meter]
-[[nodiscard]] static double FovToFocalLength(
-  const double& sensorWidth,
-  const double& fov);
-
-/// @brief Compute diagonal sensor width given focal length and FOV
-/// @param focalLength Focal length [meter]
-/// @param fov Field of view [rad]
-/// @return Sensor width [m]
-[[nodiscard]] static double SensorWidth(
-  const double& focalLength,
-  const double& fov);
+  /// \brief Convert from focal length to FOV for a rectilinear lens
+  /// \ref https://en.wikipedia.org/wiki/Focal_length
+  /// @param sensorWidth Diagonal sensor width [meter]
+  /// @param focalLength The focal length [meter]
+  /// @return The field of view [rad]
+  public: static double FocalLengthToFov(
+      double sensorWidth, double focalLength);
+
+  /// \brief Convert from FOV to focal length for a rectilinear lens
+  /// \ref https://en.wikipedia.org/wiki/Focal_length
+  /// @param sensorWidth Diagonal sensor width [meter]
+  /// @param fov The field of view [rad]
+  /// @return The focal length [meter]
+  public: static double FovToFocalLength(
+      double sensorWidth, double fov);
+
+  /// @brief Compute diagonal sensor width given focal length and FOV
+  /// @param focalLength Focal length [meter]
+  /// @param fov Field of view [rad]
+  /// @return Sensor width [m]
+  public: static double SensorWidth(
+      double focalLength, double fov);
+};
 
 //////////////////////////////////////////////////
 void CameraZoomPlugin::Impl::OnZoom(const msgs::Double &_msg)
@@ -252,6 +248,29 @@ void CameraZoomPlugin::Impl::OnRenderTeardown()
   this->isValidConfig = false;
 }
 
+//////////////////////////////////////////////////
+double CameraZoomPlugin::Impl::FocalLengthToFov(
+    double sensorWidth, double focalLength)
+{
+  return 2 * std::atan2(sensorWidth, 2 * focalLength);
+}
+
+//////////////////////////////////////////////////
+double CameraZoomPlugin::Impl::FovToFocalLength(
+    double sensorWidth, double fov)
+{
+  // This is derived from FocalLengthToFov.
+  return sensorWidth / (2 * std::tan(fov / 2));
+}
+
+//////////////////////////////////////////////////
+double CameraZoomPlugin::Impl::SensorWidth(
+    double focalLength, double fov)
+{
+  // This is derived from FocalLengthToFov.
+  return 2 * std::tan(fov / 2) * focalLength;
+}
+
 //////////////////////////////////////////////////
 //////////////////////////////////////////////////
 CameraZoomPlugin::~CameraZoomPlugin() = default;
@@ -393,7 +412,8 @@ void CameraZoomPlugin::PreUpdate(
   if (!comp)
     return;
 
-  if (this->impl->zoomChanged) {
+  if (this->impl->zoomChanged)
+  {
     // Only calculate goal once each time zoom is changed.
     const auto requestedZoomCmd = this->impl->zoomCommand.load();
     const auto clampedZoomCmd = std::clamp(requestedZoomCmd,
@@ -404,26 +424,23 @@ void CameraZoomPlugin::PreUpdate(
       gzwarn << "Requested zoom command of " << requestedZoomCmd
         << " has been clamped to " << clampedZoomCmd << ".\n";
     }
-    this->impl->goalHfov =  this->impl->curHfov / clampedZoomCmd;
+    this->impl->goalHfov =  this->impl->refHfov / clampedZoomCmd;
     this->impl->zoomChanged = false;
   }
 
-  // Goal is achieved, nothing to update.
-  if (std::abs(this->impl->goalHfov - this->impl->curHfov) <
-    std::numeric_limits<double>::epsilon())
-    return;
-
   // Update component.
   sdf::Sensor &sensor = comp->Data();
   sdf::Camera *cameraSdf = sensor.CameraSensor();
-  assert(cameraSdf);  // Halt on debug mode intentionally.
-  if (!cameraSdf) {
-    // In release mode, return before a nullptr dereference.
+  if (!cameraSdf)
     return;
-  }
 
   const auto oldHfov = cameraSdf->HorizontalFov().Radian();
 
+  // Goal is achieved, nothing to update.
+  if (std::abs(this->impl->goalHfov - oldHfov) <
+    std::numeric_limits<double>::epsilon())
+    return;
+
   // This is the amount of time passed since the last update.
   const auto dt = _info.dt;
   // How many meters the focal length could change per update loop
@@ -432,26 +449,28 @@ void CameraZoomPlugin::PreUpdate(
   const auto curFocalLength = cameraSdf->LensFocalLength();
 
   // This value should be static every iteration.
-  const auto sensorWidth = SensorWidth(curFocalLength, oldHfov);
+  const auto sensorWidth = CameraZoomPlugin::Impl::SensorWidth(
+      curFocalLength, oldHfov);
 
-  const auto goalFocalLength = FovToFocalLength(
-    sensorWidth, this->impl->goalHfov);
+  const auto goalFocalLength = CameraZoomPlugin::Impl::FovToFocalLength(
+      sensorWidth, this->impl->goalHfov);
   // How many meters the focal length should change this iteration
   const auto deltaFL = std::min(maxFocalLengthChange,
     std::abs(curFocalLength - goalFocalLength));
 
-
-
   // Update rendering camera with the latest focal length.
-  double newHfov;
-  if (goalFocalLength > curFocalLength) {
-    const auto newFocalLength =  curFocalLength + deltaFL;
-    newHfov = FocalLengthToFov(sensorWidth, newFocalLength);
-
-  } else {
-    const auto newFocalLength =  curFocalLength  - deltaFL;
-    newHfov = FocalLengthToFov(sensorWidth, newFocalLength);
+  double newFocalLength;
+  if (goalFocalLength > curFocalLength)
+  {
+    newFocalLength =  curFocalLength + deltaFL;
+  }
+  else
+  {
+    newFocalLength =  curFocalLength  - deltaFL;
   }
+
+  const auto newHfov = CameraZoomPlugin::Impl::FocalLengthToFov(
+      sensorWidth, newFocalLength);
   cameraSdf->SetHorizontalFov(newHfov);
   _ecm.SetChanged(cameraEntity, components::Camera::typeId,
     ComponentState::OneTimeChange);
@@ -460,26 +479,6 @@ void CameraZoomPlugin::PreUpdate(
   this->impl->camera->SetHFOV(newHfov);
 }
 
-//////////////////////////////////////////////////
-double FocalLengthToFov(
-  const double& sensorWidth, const double& focalLength)
-{
-  return 2 * std::atan2( sensorWidth,  2 * focalLength);
-}
-
-//////////////////////////////////////////////////
-double FovToFocalLength(
-  const double& sensorWidth, const double& fov)
-{
-  // This is derived from FocalLengthToFov.
-  return sensorWidth / ( 2 * std::tan(fov / 2));
-}
-
-double SensorWidth(const double& focalLength, const double& fov){
-  // This is derived from FocalLengthToFov.
-  return 2 * std::tan(fov / 2) * focalLength;
-}
-
 //////////////////////////////////////////////////
 void CameraZoomPlugin::PostUpdate(
     const UpdateInfo &/*_info*/,