Skip to content

Commit

Permalink
Fix camera info in depth camera
Browse files Browse the repository at this point in the history 
Signed-off-by: Alejandro Hernández Cordero <[email protected]>
  • Loading branch information
@ahcorde
ahcorde committed Nov 20, 2024
1 parent 5742263 commit ec06f07
Showing 1 changed file with 216 additions and 3 deletions.
219 changes: 216 additions & 3 deletions src/DepthCameraSensor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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*/)
{
Expand Down Expand Up @@ -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)
{
Expand All @@ -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);
Expand Down Expand Up @@ -435,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();

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

Expand Down

0 comments on commit ec06f07

Please sign in to comment.