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; } //////////////////////////////////////////////////