diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index a20e9794..b71b1229 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -43,6 +43,8 @@ #include "gz/sensors/ImageNoise.hh" #include "gz/sensors/RenderingEvents.hh" +#include + #include "PointCloudUtil.hh" // undefine near and far macros from windows.h @@ -70,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 @@ -188,6 +253,78 @@ 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*/) { @@ -367,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) { @@ -381,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); @@ -435,6 +570,39 @@ bool DepthCameraSensor::CreateCamera() this->dataPtr->depthCamera->SetAspectRatio(static_cast(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(); @@ -452,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, @@ -482,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; }