Fix camera info in depth camera

src/DepthCameraSensor.cc

@@ -43,6 +43,8 @@
 #include "gz/sensors/ImageNoise.hh"
 #include "gz/sensors/RenderingEvents.hh"
 
+#include <gz/rendering/Utils.hh>
+
 #include "PointCloudUtil.hh"
 
 // undefine near and far macros from windows.h
@@ -54,6 +56,10 @@
 /// \brief Private data for DepthCameraSensor
 class gz::sensors::DepthCameraSensorPrivate
 {
+  /// \brief Callback for triggered subscription
+  /// \param[in] _msg Boolean message
+  public: void OnTrigger(const msgs::Boolean &_msg);
+
   /// \brief Save an image
   /// \param[in] _data the image data to be saved
   /// \param[in] _width width of image in pixels
@@ -66,6 +72,69 @@ class gz::sensors::DepthCameraSensorPrivate
   public: bool SaveImage(const float *_data, unsigned int _width,
     unsigned int _height, gz::common::Image::PixelFormatType _format);
 
+  /// \brief Computes the OpenGL NDC matrix
+  /// \param[in] _left Left vertical clipping plane
+  /// \param[in] _right Right vertical clipping plane
+  /// \param[in] _bottom Bottom horizontal clipping plane
+  /// \param[in] _top Top horizontal clipping plane
+  /// \param[in] _near Distance to the nearer depth clipping plane
+  ///            This value is negative if the plane is to be behind
+  ///            the camera
+  /// \param[in] _far Distance to the farther depth clipping plane
+  ///            This value is negative if the plane is to be behind
+  ///            the camera
+  /// \return OpenGL NDC (Normalized Device Coordinates) matrix
+  public: static math::Matrix4d BuildNDCMatrix(
+          double _left, double _right,
+          double _bottom, double _top,
+          double _near, double _far);
+
+  /// \brief Computes the OpenGL perspective matrix
+  /// \param[in] _intrinsicsFx Horizontal focal length (in pixels)
+  /// \param[in] _intrinsicsFy Vertical focal length (in pixels)
+  /// \param[in] _intrinsicsCx X coordinate of principal point in pixels
+  /// \param[in] _intrinsicsCy Y coordinate of principal point in pixels
+  /// \param[in] _intrinsicsS Skew coefficient defining the angle between
+  ///            the x and y pixel axes
+  /// \param[in] _clipNear Distance to the nearer depth clipping plane
+  ///            This value is negative if the plane is to be behind
+  ///            the camera
+  /// \param[in] _clipFar Distance to the farther depth clipping plane
+  ///            This value is negative if the plane is to be behind
+  ///            the camera
+  /// \return OpenGL perspective matrix
+  public: static math::Matrix4d BuildPerspectiveMatrix(
+          double _intrinsicsFx, double _intrinsicsFy,
+          double _intrinsicsCx, double _intrinsicsCy,
+          double _intrinsicsS,
+          double _clipNear, double _clipFar);
+
+  /// \brief Computes the OpenGL projection matrix by multiplying
+  ///        the OpenGL Normalized Device Coordinates matrix (NDC) with
+  ///        the OpenGL perspective matrix
+  ///        openglProjectionMatrix = ndcMatrix * perspectiveMatrix
+  /// \param[in] _imageWidth Image width (in pixels)
+  /// \param[in] _imageHeight Image height (in pixels)
+  /// \param[in] _intrinsicsFx Horizontal focal length (in pixels)
+  /// \param[in] _intrinsicsFy Vertical focal length (in pixels)
+  /// \param[in] _intrinsicsCx X coordinate of principal point in pixels
+  /// \param[in] _intrinsicsCy Y coordinate of principal point in pixels
+  /// \param[in] _intrinsicsS Skew coefficient defining the angle between
+  ///             the x and y pixel axes
+  /// \param[in] _clipNear Distance to the nearer depth clipping plane
+  ///            This value is negative if the plane is to be behind
+  ///            the camera
+  /// \param[in] _clipFar Distance to the farther depth clipping plane
+  ///            This value is negative if the plane is to be behind
+  ///            the camera
+  /// \return OpenGL projection matrix
+  public: static math::Matrix4d BuildProjectionMatrix(
+          double _imageWidth, double _imageHeight,
+          double _intrinsicsFx, double _intrinsicsFy,
+          double _intrinsicsCx, double _intrinsicsCy,
+          double _intrinsicsS,
+          double _clipNear, double _clipFar);
+
   /// \brief Helper function to convert depth data to depth image
   /// \param[in] _data depth data
   /// \param[out] _imageBuffer resulting depth image data
@@ -121,6 +190,15 @@ class gz::sensors::DepthCameraSensorPrivate
   /// \brief Just a mutex for thread safety
   public: std::mutex mutex;
 
+  /// \brief True if camera is triggered by a topic
+  public: bool isTriggeredCamera = false;
+
+  /// \brief True if camera has been triggered by a topic
+  public: bool isTriggered = false;
+
+  /// \brief Topic for camera trigger
+  public: std::string triggerTopic = "";
+
   /// \brief True to save images
   public: bool saveImage = false;
 
@@ -175,6 +253,85 @@ bool DepthCameraSensorPrivate::ConvertDepthToImage(
   return true;
 }
 
+
+//////////////////////////////////////////////////
+math::Matrix4d DepthCameraSensorPrivate::BuildProjectionMatrix(
+    double _imageWidth, double _imageHeight,
+    double _intrinsicsFx, double _intrinsicsFy,
+    double _intrinsicsCx, double _intrinsicsCy,
+    double _intrinsicsS,
+    double _clipNear, double _clipFar)
+{
+  return DepthCameraSensorPrivate::BuildNDCMatrix(
+           0, _imageWidth, 0, _imageHeight, _clipNear, _clipFar) *
+           DepthCameraSensorPrivate::BuildPerspectiveMatrix(
+             _intrinsicsFx, _intrinsicsFy,
+             _intrinsicsCx, _imageHeight - _intrinsicsCy,
+             _intrinsicsS, _clipNear, _clipFar);
+}
+
+//////////////////////////////////////////////////
+math::Matrix4d DepthCameraSensorPrivate::BuildNDCMatrix(
+    double _left, double _right,
+    double _bottom, double _top,
+    double _near, double _far)
+{
+  double inverseWidth = 1.0 / (_right - _left);
+  double inverseHeight = 1.0 / (_top - _bottom);
+  double inverseDistance = 1.0 / (_far - _near);
+
+  return math::Matrix4d(
+           2.0 * inverseWidth,
+           0.0,
+           0.0,
+           -(_right + _left) * inverseWidth,
+           0.0,
+           2.0 * inverseHeight,
+           0.0,
+           -(_top + _bottom) * inverseHeight,
+           0.0,
+           0.0,
+           -2.0 * inverseDistance,
+           -(_far + _near) * inverseDistance,
+           0.0,
+           0.0,
+           0.0,
+           1.0);
+}
+
+//////////////////////////////////////////////////
+math::Matrix4d DepthCameraSensorPrivate::BuildPerspectiveMatrix(
+    double _intrinsicsFx, double _intrinsicsFy,
+    double _intrinsicsCx, double _intrinsicsCy,
+    double _intrinsicsS,
+    double _clipNear, double _clipFar)
+{
+  return math::Matrix4d(
+           _intrinsicsFx,
+           _intrinsicsS,
+           -_intrinsicsCx,
+           0.0,
+           0.0,
+           _intrinsicsFy,
+           -_intrinsicsCy,
+           0.0,
+           0.0,
+           0.0,
+           _clipNear + _clipFar,
+           _clipNear * _clipFar,
+           0.0,
+           0.0,
+           -1.0,
+           0.0);
+}
+
+//////////////////////////////////////////////////
+void DepthCameraSensorPrivate::OnTrigger(const msgs::Boolean &/*_msg*/)
+{
+  std::lock_guard<std::mutex> lock(this->mutex);
+  this->isTriggered = true;
+}
+
 //////////////////////////////////////////////////
 bool DepthCameraSensorPrivate::SaveImage(const float *_data,
     unsigned int _width, unsigned int _height,
@@ -287,6 +444,32 @@ bool DepthCameraSensor::Load(const sdf::Sensor &_sdf)
   igndbg << "Depth images for [" << this->Name() << "] advertised on ["
          << this->Topic() << "]" << std::endl;
 
+  if (_sdf.CameraSensor()->Triggered())
+  {
+    if (!_sdf.CameraSensor()->TriggerTopic().empty())
+    {
+      this->dataPtr->triggerTopic = _sdf.CameraSensor()->TriggerTopic();
+    }
+    else
+    {
+      this->dataPtr->triggerTopic =
+          transport::TopicUtils::AsValidTopic(this->dataPtr->triggerTopic);
+
+      if (this->dataPtr->triggerTopic.empty())
+      {
+        ignerr << "Invalid trigger topic name" << std::endl;
+        return false;
+      }
+    }
+
+    this->dataPtr->node.Subscribe(this->dataPtr->triggerTopic,
+        &DepthCameraSensorPrivate::OnTrigger, this->dataPtr.get());
+
+    igndbg << "Camera trigger messages for [" << this->Name() << "] subscribed"
+           << " on [" << this->dataPtr->triggerTopic << "]" << std::endl;
+    this->dataPtr->isTriggeredCamera = true;
+  }
+
   if (!this->AdvertiseInfo())
     return false;
 
@@ -321,7 +504,7 @@ bool DepthCameraSensor::Load(const sdf::Sensor &_sdf)
 //////////////////////////////////////////////////
 bool DepthCameraSensor::CreateCamera()
 {
-  const sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor();
+  sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor();
 
   if (!cameraSdf)
   {
@@ -335,8 +518,6 @@ bool DepthCameraSensor::CreateCamera()
   double far = cameraSdf->FarClip();
   double near = cameraSdf->NearClip();
 
-  this->PopulateInfo(cameraSdf);
-
   this->dataPtr->depthCamera = this->Scene()->CreateDepthCamera(
       this->Name());
   this->dataPtr->depthCamera->SetImageWidth(width);
@@ -389,6 +570,39 @@ bool DepthCameraSensor::CreateCamera()
   this->dataPtr->depthCamera->SetAspectRatio(static_cast<double>(width)/height);
   this->dataPtr->depthCamera->SetHFOV(angle);
 
+  // Update the DOM object intrinsics to have consistent
+  // intrinsics between ogre camera and camera_info msg
+  if(!cameraSdf->HasLensIntrinsics())
+  {
+    auto intrinsicMatrix =
+      gz::rendering::projectionToCameraIntrinsic(
+        this->dataPtr->depthCamera->ProjectionMatrix(),
+        this->dataPtr->depthCamera->ImageWidth(),
+        this->dataPtr->depthCamera->ImageHeight()
+      );
+
+    cameraSdf->SetLensIntrinsicsFx(intrinsicMatrix(0, 0));
+    cameraSdf->SetLensIntrinsicsFy(intrinsicMatrix(1, 1));
+    cameraSdf->SetLensIntrinsicsCx(intrinsicMatrix(0, 2));
+    cameraSdf->SetLensIntrinsicsCy(intrinsicMatrix(1, 2));
+  }
+  // set custom projection matrix based on intrinsics param specified in sdf
+  else
+  {
+    double fx = cameraSdf->LensIntrinsicsFx();
+    double fy = cameraSdf->LensIntrinsicsFy();
+    double cx = cameraSdf->LensIntrinsicsCx();
+    double cy = cameraSdf->LensIntrinsicsCy();
+    double s = cameraSdf->LensIntrinsicsSkew();
+    auto projectionMatrix = DepthCameraSensorPrivate::BuildProjectionMatrix(
+        this->dataPtr->depthCamera->ImageWidth(),
+        this->dataPtr->depthCamera->ImageHeight(),
+        fx, fy, cx, cy, s,
+        this->dataPtr->depthCamera->NearClipPlane(),
+        this->dataPtr->depthCamera->FarClipPlane());
+    this->dataPtr->depthCamera->SetProjectionMatrix(projectionMatrix);
+  }
+
   // Create depth texture when the camera is reconfigured from default values
   this->dataPtr->depthCamera->CreateDepthTexture();
 
@@ -406,6 +620,48 @@ bool DepthCameraSensor::CreateCamera()
     this->dataPtr->saveImage = true;
   }
 
+  // Update the DOM object intrinsics to have consistent
+  // projection matrix values between ogre camera and camera_info msg
+  // If these values are not defined in the SDF then we need to update
+  // these values to something reasonable. The projection matrix is
+  // the cumulative effect of intrinsic and extrinsic parameters
+  if(!cameraSdf->HasLensProjection())
+  {
+    // Note that the matrix from Ogre via camera->ProjectionMatrix() has a
+    // different format than the projection matrix used in SDFormat.
+    // This is why they are converted using projectionToCameraIntrinsic.
+    // The resulting matrix is the intrinsic matrix, but since the user has
+    // not overridden the values, this is also equal to the projection matrix.
+    auto intrinsicMatrix =
+      gz::rendering::projectionToCameraIntrinsic(
+        this->dataPtr->depthCamera->ProjectionMatrix(),
+        this->dataPtr->depthCamera->ImageWidth(),
+        this->dataPtr->depthCamera->ImageHeight()
+      );
+    cameraSdf->SetLensProjectionFx(intrinsicMatrix(0, 0));
+    cameraSdf->SetLensProjectionFy(intrinsicMatrix(1, 1));
+    cameraSdf->SetLensProjectionCx(intrinsicMatrix(0, 2));
+    cameraSdf->SetLensProjectionCy(intrinsicMatrix(1, 2));
+  }
+  // set custom projection matrix based on projection param specified in sdf
+  else
+  {
+    // tx and ty are not used
+    double fx = cameraSdf->LensProjectionFx();
+    double fy = cameraSdf->LensProjectionFy();
+    double cx = cameraSdf->LensProjectionCx();
+    double cy = cameraSdf->LensProjectionCy();
+    double s = 0;
+
+    auto projectionMatrix = DepthCameraSensorPrivate::BuildProjectionMatrix(
+        this->dataPtr->depthCamera->ImageWidth(),
+        this->dataPtr->depthCamera->ImageHeight(),
+        fx, fy, cx, cy, s,
+        this->dataPtr->depthCamera->NearClipPlane(),
+        this->dataPtr->depthCamera->FarClipPlane());
+    this->dataPtr->depthCamera->SetProjectionMatrix(projectionMatrix);
+  }
+
   this->dataPtr->depthConnection =
       this->dataPtr->depthCamera->ConnectNewDepthFrame(
       std::bind(&DepthCameraSensor::OnNewDepthFrame, this,
@@ -436,6 +692,9 @@ bool DepthCameraSensor::CreateCamera()
   this->dataPtr->pointMsg.set_row_step(
       this->dataPtr->pointMsg.point_step() * this->ImageWidth());
 
+  // Populate camera info topic
+  this->PopulateInfo(cameraSdf);
+
   return true;
 }
 
@@ -536,6 +795,13 @@ bool DepthCameraSensor::Update(
     this->PublishInfo(_now);
   }
 
+  // render only if necessary
+  if (this->dataPtr->isTriggeredCamera &&
+      !this->dataPtr->isTriggered)
+  {
+    return true;
+  }
+
   if (!this->HasDepthConnections() && !this->HasPointConnections())
   {
     return false;
@@ -620,6 +886,12 @@ bool DepthCameraSensor::Update(
     this->AddSequence(this->dataPtr->pointMsg.mutable_header(), "pointMsg");
     this->dataPtr->pointPub.Publish(this->dataPtr->pointMsg);
   }
+
+  if (this->dataPtr->isTriggeredCamera)
+  {
+    return this->dataPtr->isTriggered = false;
+  }
+
   return true;
 }