diff --git a/models/gimbal_small_3d/model.sdf b/models/gimbal_small_3d/model.sdf
index ada294df..19b7805c 100644
--- a/models/gimbal_small_3d/model.sdf
+++ b/models/gimbal_small_3d/model.sdf
@@ -207,7 +207,8 @@
- 15.0
+ 125.0
+ 0.42514285714
diff --git a/src/CameraZoomPlugin.cc b/src/CameraZoomPlugin.cc
index 0a16ba48..f2c2d29a 100644
--- a/src/CameraZoomPlugin.cc
+++ b/src/CameraZoomPlugin.cc
@@ -17,7 +17,10 @@
#include "CameraZoomPlugin.hh"
+#include
#include
+#include
+#include
#include
#include
@@ -99,7 +102,6 @@ class CameraZoomPlugin::Impl
return std::optional(parent->Data());
}
-
/// \brief World occupied by the parent model.
public: World world{kNullEntity};
@@ -123,15 +125,22 @@ class CameraZoomPlugin::Impl
/// \brief Value of the most recently received zoom command.
public: std::atomic zoomCommand{1.0};
- /// \brief Current horizontal field of view (radians).
- public: double hfov{2.0};
+ /// \brief Reference horizontal field of view (radians).
+ public: double refHfov{2.0};
+
+ /// \brief Goal horizontal field of view (radians).
+ public: std::atomic goalHfov{2.0};
- /// \brief Zoom factor.
- public: double zoom{1.0};
+ /// \brief Current zoom factor.
+ public: double curZoom{1.0};
/// \brief Maximum zoom factor.
public: double maxZoom{10.0};
+ /// \brief Slew rate (meters change in focal length per second).
+ /// Default: infinity, which causes instant changes in focal length.
+ public: double slewRate{std::numeric_limits::infinity()};
+
/// \brief Minimum zoom factor == 1.0.
public: static constexpr double minZoom{1.0};
@@ -152,6 +161,29 @@ 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]
+ 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);
};
//////////////////////////////////////////////////
@@ -215,6 +247,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;
@@ -294,6 +349,10 @@ void CameraZoomPlugin::Configure(
{
this->impl->maxZoom = _sdf->Get("max_zoom");
}
+ if (_sdf->HasElement("slew_rate"))
+ {
+ this->impl->slewRate = _sdf->Get("slew_rate");
+ }
// Configure zoom command topic.
{
@@ -330,7 +389,7 @@ void CameraZoomPlugin::Configure(
//////////////////////////////////////////////////
void CameraZoomPlugin::PreUpdate(
- const UpdateInfo &/*_info*/,
+ const UpdateInfo &_info,
EntityComponentManager &_ecm)
{
GZ_PROFILE("CameraZoomPlugin::PreUpdate");
@@ -352,31 +411,76 @@ void CameraZoomPlugin::PreUpdate(
if (!comp)
return;
- if (!this->impl->zoomChanged)
- return;
+ 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,
+ this->impl->minZoom, this->impl->maxZoom);
+ if (std::abs(requestedZoomCmd - clampedZoomCmd) >
+ std::numeric_limits::epsilon())
+ {
+ gzwarn << "Requested zoom command of " << requestedZoomCmd
+ << " has been clamped to " << clampedZoomCmd << ".\n";
+ }
+ this->impl->goalHfov = this->impl->refHfov / clampedZoomCmd;
+ this->impl->zoomChanged = false;
+ }
// Update component.
sdf::Sensor &sensor = comp->Data();
sdf::Camera *cameraSdf = sensor.CameraSensor();
+ 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::epsilon())
+ return;
+
+ const auto curFocalLength = cameraSdf->LensFocalLength();
+
+ // This value should be static every iteration.
+ const auto sensorWidth = CameraZoomPlugin::Impl::SensorWidth(
+ curFocalLength, oldHfov);
+ const auto goalFocalLength = CameraZoomPlugin::Impl::FovToFocalLength(
+ sensorWidth, this->impl->goalHfov);
- this->impl->zoom = std::max(std::min(this->impl->zoomCommand.load(),
- this->impl->maxZoom), this->impl->minZoom);
- math::Angle oldHfov = cameraSdf->HorizontalFov();
- math::Angle newHfov = this->impl->hfov / this->impl->zoom;
+ double newFocalLength;
+ if (std::isfinite(this->impl->slewRate)) {
+ // 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
+ const auto maxFocalLengthChange = this->impl->slewRate *
+ std::chrono::duration(dt).count();
+ // How many meters the focal length should change this iteration
+ const auto deltaFL = std::min(maxFocalLengthChange,
+ std::abs(curFocalLength - goalFocalLength));
+
+ if (goalFocalLength > curFocalLength)
+ {
+ newFocalLength = curFocalLength + deltaFL;
+ }
+ else
+ {
+ newFocalLength = curFocalLength - deltaFL;
+ }
+ } else {
+ newFocalLength = goalFocalLength;
+ }
+
+ const auto newHfov = CameraZoomPlugin::Impl::FocalLengthToFov(
+ sensorWidth, newFocalLength);
+ // Update rendering camera with the latest focal length.
cameraSdf->SetHorizontalFov(newHfov);
_ecm.SetChanged(cameraEntity, components::Camera::typeId,
- ComponentState::OneTimeChange);
+ ComponentState::OneTimeChange);
// Update rendering camera.
this->impl->camera->SetHFOV(newHfov);
-
- gzdbg << "CameraZoomPlugin:\n"
- << "Zoom: " << this->impl->zoom << "\n"
- << "Old HFOV: " << oldHfov << " rad\n"
- << "New HFOV: " << newHfov << " rad\n";
-
- this->impl->zoomChanged = false;
}
//////////////////////////////////////////////////