Skip to content

Commit

Permalink
Camera: ensure zoom returns to initial position
Browse files Browse the repository at this point in the history 
Signed-off-by: Rhys Mainwaring <[email protected]>
  • Loading branch information
@srmainwaring
srmainwaring committed Jan 25, 2024
1 parent 59869aa commit 9f34a15
Showing 1 changed file with 72 additions and 73 deletions.
145 changes: 72 additions & 73 deletions src/CameraZoomPlugin.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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};

Expand All @@ -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};
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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,
Expand All @@ -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
Expand All @@ -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);
Expand All @@ -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*/,
Expand Down

0 comments on commit 9f34a15

Please sign in to comment.