Set lens intrinsics in Depth and Rgbd camera sensors

src/CMakeLists.txt

@@ -1,5 +1,6 @@
 set (sources
   BrownDistortionModel.cc
+  CameraSensorUtil.cc
   Distortion.cc
   EnvironmentalData.cc
   GaussianNoiseModel.cc

src/CameraSensor.cc

@@ -31,6 +31,7 @@
 #include <gz/msgs/Utility.hh>
 #include <gz/transport/Node.hh>
 
+#include "CameraSensorUtil.hh"
 #include "gz/sensors/CameraSensor.hh"
 #include "gz/sensors/ImageBrownDistortionModel.hh"
 #include "gz/sensors/ImageDistortion.hh"
@@ -61,69 +62,6 @@ class gz::sensors::CameraSensorPrivate
   public: bool SaveImage(const unsigned char *_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 node to create publisher
   public: transport::Node node;
 
@@ -320,7 +258,7 @@ bool CameraSensor::CreateCamera()
     double cx = cameraSdf->LensIntrinsicsCx();
     double cy = cameraSdf->LensIntrinsicsCy();
     double s = cameraSdf->LensIntrinsicsSkew();
-    auto projectionMatrix = CameraSensorPrivate::BuildProjectionMatrix(
+    auto projectionMatrix = BuildProjectionMatrix(
         this->dataPtr->camera->ImageWidth(),
         this->dataPtr->camera->ImageHeight(),
         fx, fy, cx, cy, s,
@@ -374,7 +312,7 @@ bool CameraSensor::CreateCamera()
     double cy = cameraSdf->LensProjectionCy();
     double s = 0;
 
-    auto projectionMatrix = CameraSensorPrivate::BuildProjectionMatrix(
+    auto projectionMatrix = BuildProjectionMatrix(
         this->dataPtr->camera->ImageWidth(),
         this->dataPtr->camera->ImageHeight(),
         fx, fy, cx, cy, s,
@@ -919,74 +857,3 @@ bool CameraSensor::HasInfoConnections() const
 {
   return this->dataPtr->infoPub && this->dataPtr->infoPub.HasConnections();
 }
-
-//////////////////////////////////////////////////
-math::Matrix4d CameraSensorPrivate::BuildProjectionMatrix(
-    double _imageWidth, double _imageHeight,
-    double _intrinsicsFx, double _intrinsicsFy,
-    double _intrinsicsCx, double _intrinsicsCy,
-    double _intrinsicsS,
-    double _clipNear, double _clipFar)
-{
-  return CameraSensorPrivate::BuildNDCMatrix(
-           0, _imageWidth, 0, _imageHeight, _clipNear, _clipFar) *
-           CameraSensorPrivate::BuildPerspectiveMatrix(
-             _intrinsicsFx, _intrinsicsFy,
-             _intrinsicsCx, _imageHeight - _intrinsicsCy,
-             _intrinsicsS, _clipNear, _clipFar);
-}
-
-//////////////////////////////////////////////////
-math::Matrix4d CameraSensorPrivate::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 CameraSensorPrivate::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);
-}

src/CameraSensorUtil.cc

@@ -0,0 +1,91 @@
+/*
+ * Copyright (C) 2023 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include "CameraSensorUtil.hh"
+
+#include <gz/math/Matrix4.hh>
+
+//////////////////////////////////////////////////
+gz::math::Matrix4d gz::sensors::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);
+}
+
+//////////////////////////////////////////////////
+gz::math::Matrix4d gz::sensors::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);
+}
+
+//////////////////////////////////////////////////
+gz::math::Matrix4d gz::sensors::BuildProjectionMatrix(
+    double _imageWidth, double _imageHeight,
+    double _intrinsicsFx, double _intrinsicsFy,
+    double _intrinsicsCx, double _intrinsicsCy,
+    double _intrinsicsS,
+    double _clipNear, double _clipFar)
+{
+  return BuildNDCMatrix(
+           0, _imageWidth, 0, _imageHeight, _clipNear, _clipFar) *
+         BuildPerspectiveMatrix(
+           _intrinsicsFx, _intrinsicsFy,
+           _intrinsicsCx, _imageHeight - _intrinsicsCy,
+           _intrinsicsS, _clipNear, _clipFar);
+}

src/CameraSensorUtil.hh

@@ -0,0 +1,95 @@
+/*
+ * Copyright (C) 2023 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef GZ_SENSORS_CAMERASENSORUTIL_HH_
+#define GZ_SENSORS_CAMERASENSORUTIL_HH_
+
+#include <gz/math/Matrix4.hh>
+
+#include "gz/sensors/config.hh"
+#include "gz/sensors/Export.hh"
+
+namespace gz
+{
+namespace sensors
+{
+// Inline bracket to help doxygen filtering.
+inline namespace GZ_SENSORS_VERSION_NAMESPACE {
+/// \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
+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
+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
+math::Matrix4d BuildProjectionMatrix(double _imageWidth, double _imageHeight,
+                                     double _intrinsicsFx, double _intrinsicsFy,
+                                     double _intrinsicsCx, double _intrinsicsCy,
+                                     double _intrinsicsS, double _clipNear,
+                                     double _clipFar);
+
+}
+}
+}
+
+#endif