Skip to content

Commit

Permalink
Set custom projection matrix based on intrinsics params from SDF (gaz…
Browse files Browse the repository at this point in the history 
…ebosim#293)

Closes gazebosim#288

This PR extends the camera sensor functionality to support custom projection matrix based on input instrinics values. Specifically, it reads the lens <instrinsics> params from SDF, builds the projection matrix from the input values using code ported from Gazebo-classic, and set the camera to use this custom projection matrix.

Expanded the camera intrinsics integration test to compare image output from cameras with different intrinsics values.

Signed-off-by: Ian Chen <[email protected]>
  • Loading branch information
@iche033
iche033 authored Dec 7, 2022
1 parent 306e92a commit 2ec0200
Show file tree
Hide file tree
Showing 3 changed files with 382 additions and 36 deletions.
174 changes: 162 additions & 12 deletions src/CameraSensor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,69 @@ 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;

Expand Down Expand Up @@ -225,18 +288,6 @@ bool CameraSensor::CreateCamera()
break;
}

this->dataPtr->image = this->dataPtr->camera->CreateImage();

this->Scene()->RootVisual()->AddChild(this->dataPtr->camera);

// Create the directory to store frames
if (cameraSdf->SaveFrames())
{
this->dataPtr->saveImagePath = cameraSdf->SaveFramesPath();
this->dataPtr->saveImagePrefix = this->Name() + "_";
this->dataPtr->saveImage = true;
}

// Update the DOM object intrinsics to have consistent
// intrinsics between ogre camera and camera_info msg
if(!cameraSdf->HasLensIntrinsics())
Expand All @@ -253,6 +304,34 @@ bool CameraSensor::CreateCamera()
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->dataPtr->image = this->dataPtr->camera->CreateImage();

this->Scene()->RootVisual()->AddChild(this->dataPtr->camera);

// Create the directory to store frames
if (cameraSdf->SaveFrames())
{
this->dataPtr->saveImagePath = cameraSdf->SaveFramesPath();
this->dataPtr->saveImagePrefix = this->Name() + "_";
this->dataPtr->saveImage = true;
}

// Populate camera info topic
this->PopulateInfo(cameraSdf);
Expand Down Expand Up @@ -746,3 +825,74 @@ bool CameraSensor::HasConnections() const
return (this->dataPtr->pub && this->dataPtr->pub.HasConnections()) ||
this->dataPtr->imageEvent.ConnectionCount() > 0u;
}

//////////////////////////////////////////////////
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);
}
Loading

0 comments on commit 2ec0200

Please sign in to comment.