Set camera projection matrix based on lens params for other types of cameras

include/gz/sensors/CameraSensor.hh

@@ -148,6 +148,17 @@ namespace gz
       /// \return The camera optical frame
       public: const std::string& OpticalFrameId() const;
 
+      /// \brief Set camera lens intrinsics and projection based on
+      /// values from SDF. If the camera SDF does not contain intrinsic or
+      /// projection parameters, the camera will not be updated. Instead, the
+      /// camera SDF will be updated with intrinsic and projection values
+      /// computed manually from current camera intrinsic properties.
+      /// \param[in] _camera Camera to set intrinsic and projection params.
+      /// \param[in,out] _cameraSdf Camera sdf with intrinisc and projection
+      /// parameters.
+      public: void UpdateLensIntrinsicsAndProjection(
+          rendering::CameraPtr _camera, sdf::Camera &_cameraSdf);
+
       /// \brief Advertise camera info topic.
       /// \return True if successful.
       protected: bool AdvertiseInfo();

src/BoundingBoxCameraSensor.cc

@@ -303,9 +303,6 @@ bool BoundingBoxCameraSensor::CreateCamera()
     return false;
   }
 
-  // Camera Info Msg
-  this->PopulateInfo(sdfCamera);
-
   if (!this->dataPtr->rgbCamera)
   {
     // Create rendering camera
@@ -356,6 +353,14 @@ bool BoundingBoxCameraSensor::CreateCamera()
   this->AddSensor(this->dataPtr->boundingboxCamera);
   this->AddSensor(this->dataPtr->rgbCamera);
 
+  this->UpdateLensIntrinsicsAndProjection(this->dataPtr->rgbCamera,
+      *sdfCamera);
+  this->UpdateLensIntrinsicsAndProjection(this->dataPtr->boundingboxCamera,
+      *sdfCamera);
+
+  // Camera Info Msg
+  this->PopulateInfo(sdfCamera);
+
   // Create the directory to store frames
   if (sdfCamera->SaveFrames())
   {

src/CameraSensor.cc

@@ -297,38 +297,8 @@ bool CameraSensor::CreateCamera()
       break;
   }
 
-  // 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->camera->ProjectionMatrix(),
-        this->dataPtr->camera->ImageWidth(),
-        this->dataPtr->camera->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 = CameraSensorPrivate::BuildProjectionMatrix(
-        this->dataPtr->camera->ImageWidth(),
-        this->dataPtr->camera->ImageHeight(),
-        fx, fy, cx, cy, s,
-        this->dataPtr->camera->NearClipPlane(),
-        this->dataPtr->camera->FarClipPlane());
-    this->dataPtr->camera->SetProjectionMatrix(projectionMatrix);
-  }
+  this->UpdateLensIntrinsicsAndProjection(this->dataPtr->camera,
+      *cameraSdf);
 
   this->dataPtr->image = this->dataPtr->camera->CreateImage();
 
@@ -342,48 +312,6 @@ bool CameraSensor::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->camera->ProjectionMatrix(),
-        this->dataPtr->camera->ImageWidth(),
-        this->dataPtr->camera->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 = CameraSensorPrivate::BuildProjectionMatrix(
-        this->dataPtr->camera->ImageWidth(),
-        this->dataPtr->camera->ImageHeight(),
-        fx, fy, cx, cy, s,
-        this->dataPtr->camera->NearClipPlane(),
-        this->dataPtr->camera->FarClipPlane());
-    this->dataPtr->camera->SetProjectionMatrix(projectionMatrix);
-  }
-
   // Populate camera info topic
   this->PopulateInfo(cameraSdf);
 
@@ -927,6 +855,86 @@ const std::string& CameraSensor::OpticalFrameId() const
   return this->dataPtr->opticalFrameId;
 }
 
+//////////////////////////////////////////////////
+void CameraSensor::UpdateLensIntrinsicsAndProjection(
+  rendering::CameraPtr _camera, sdf::Camera &_cameraSdf)
+{
+  // Update the DOM object intrinsics to have consistent
+  // intrinsics between ogre camera and camera_info msg
+  if(!_cameraSdf.HasLensIntrinsics())
+  {
+    auto intrinsicMatrix =
+      gz::rendering::projectionToCameraIntrinsic(
+        _camera->ProjectionMatrix(),
+        _camera->ImageWidth(),
+        _camera->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 = CameraSensorPrivate::BuildProjectionMatrix(
+        _camera->ImageWidth(),
+        _camera->ImageHeight(),
+        fx, fy, cx, cy, s,
+        _camera->NearClipPlane(),
+        _camera->FarClipPlane());
+    _camera->SetProjectionMatrix(projectionMatrix);
+  }
+
+  // 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(
+        _camera->ProjectionMatrix(),
+        _camera->ImageWidth(),
+        _camera->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 = CameraSensorPrivate::BuildProjectionMatrix(
+        _camera->ImageWidth(),
+        _camera->ImageHeight(),
+        fx, fy, cx, cy, s,
+        _camera->NearClipPlane(),
+        _camera->FarClipPlane());
+    _camera->SetProjectionMatrix(projectionMatrix);
+  }
+}
+
 //////////////////////////////////////////////////
 math::Matrix4d CameraSensorPrivate::BuildProjectionMatrix(
     double _imageWidth, double _imageHeight,

src/DepthCameraSensor.cc

@@ -316,7 +316,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)
   {
@@ -330,7 +330,6 @@ bool DepthCameraSensor::CreateCamera()
   double far = cameraSdf->FarClip();
   double near = cameraSdf->NearClip();
 
-  this->PopulateInfo(cameraSdf);
 
   this->dataPtr->depthCamera = this->Scene()->CreateDepthCamera(
       this->Name());
@@ -393,6 +392,9 @@ bool DepthCameraSensor::CreateCamera()
 
   this->Scene()->RootVisual()->AddChild(this->dataPtr->depthCamera);
 
+  this->UpdateLensIntrinsicsAndProjection(this->dataPtr->depthCamera,
+      *cameraSdf);
+
   // Create the directory to store frames
   if (cameraSdf->SaveFrames())
   {
@@ -401,6 +403,8 @@ bool DepthCameraSensor::CreateCamera()
     this->dataPtr->saveImage = true;
   }
 
+  this->PopulateInfo(cameraSdf);
+
   this->dataPtr->depthConnection =
       this->dataPtr->depthCamera->ConnectNewDepthFrame(
       std::bind(&DepthCameraSensor::OnNewDepthFrame, this,

src/RgbdCameraSensor.cc

@@ -266,16 +266,14 @@ bool RgbdCameraSensor::Load(const sdf::Sensor &_sdf)
 //////////////////////////////////////////////////
 bool RgbdCameraSensor::CreateCameras()
 {
-  const sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor();
+  sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor();
 
   if (!cameraSdf)
   {
     gzerr << "Unable to access camera SDF element\n";
     return false;
   }
 
-  this->PopulateInfo(cameraSdf);
-
   int width = cameraSdf->ImageWidth();
   int height = cameraSdf->ImageHeight();
 
@@ -368,6 +366,11 @@ bool RgbdCameraSensor::CreateCameras()
 
   this->Scene()->RootVisual()->AddChild(this->dataPtr->depthCamera);
 
+  this->UpdateLensIntrinsicsAndProjection(this->dataPtr->depthCamera,
+      *cameraSdf);
+
+  this->PopulateInfo(cameraSdf);
+
   this->dataPtr->depthConnection =
       this->dataPtr->depthCamera->ConnectNewDepthFrame(
         std::bind(&RgbdCameraSensorPrivate::OnNewDepthFrame,

src/SegmentationCameraSensor.cc

@@ -264,7 +264,7 @@ void SegmentationCameraSensor::SetScene(
 /////////////////////////////////////////////////
 bool SegmentationCameraSensor::CreateCamera()
 {
-  const auto sdfCamera = this->dataPtr->sdfSensor.CameraSensor();
+  auto sdfCamera = this->dataPtr->sdfSensor.CameraSensor();
   if (!sdfCamera)
   {
     gzerr << "Unable to access camera SDF element\n";
@@ -293,9 +293,6 @@ bool SegmentationCameraSensor::CreateCamera()
     }
   }
 
-  // Camera Info Msg
-  this->PopulateInfo(sdfCamera);
-
   // Save frames properties
   if (sdfCamera->SaveFrames())
   {
@@ -368,6 +365,9 @@ bool SegmentationCameraSensor::CreateCamera()
   // Add the rendering sensor to handle its render
   this->AddSensor(this->dataPtr->camera);
 
+  this->UpdateLensIntrinsicsAndProjection(this->dataPtr->camera,
+      *sdfCamera);
+
   // Add the rgb camera only if we want to generate dataset / save samples
   if (this->dataPtr->saveSamples)
   {
@@ -385,8 +385,14 @@ bool SegmentationCameraSensor::CreateCamera()
     this->Scene()->RootVisual()->AddChild(this->dataPtr->rgbCamera);
     this->AddSensor(this->dataPtr->rgbCamera);
     this->dataPtr->image = this->dataPtr->rgbCamera->CreateImage();
+
+    this->UpdateLensIntrinsicsAndProjection(this->dataPtr->rgbCamera,
+      *sdfCamera);
   }
 
+  // Camera Info Msg
+  this->PopulateInfo(sdfCamera);
+
   // Connection to receive the segmentation buffer
   this->dataPtr->newSegmentationConnection =
       this->dataPtr->camera->ConnectNewSegmentationFrame(

src/ThermalCameraSensor.cc

@@ -236,7 +236,7 @@ bool ThermalCameraSensor::Load(const sdf::Sensor &_sdf)
 //////////////////////////////////////////////////
 bool ThermalCameraSensor::CreateCamera()
 {
-  const sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor();
+  sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor();
 
   if (!cameraSdf)
   {
@@ -252,8 +252,6 @@ bool ThermalCameraSensor::CreateCamera()
   double farPlane = cameraSdf->FarClip();
   double nearPlane = cameraSdf->NearClip();
 
-  this->PopulateInfo(cameraSdf);
-
   this->dataPtr->thermalCamera = this->Scene()->CreateThermalCamera(
       this->Name());
   this->dataPtr->thermalCamera->SetImageWidth(width);
@@ -335,6 +333,11 @@ bool ThermalCameraSensor::CreateCamera()
 
   this->Scene()->RootVisual()->AddChild(this->dataPtr->thermalCamera);
 
+  this->UpdateLensIntrinsicsAndProjection(this->dataPtr->thermalCamera,
+      *cameraSdf);
+
+  this->PopulateInfo(cameraSdf);
+
   // Create the directory to store frames
   if (cameraSdf->SaveFrames())
   {