Skip to content

Commit 661a40a

Browse files
iche033iche033
authored
Set camera projection matrix based on lens params for other types of cameras (#432)
 --------- Signed-off-by: Ian Chen <ichen@openrobotics.org>
1 parent 930a4a0 commit 661a40a

10 files changed

+658
-89
lines changed

‎include/gz/sensors/CameraSensor.hh

+11
Original file line numberDiff line numberDiff line change
@@ -148,6 +148,17 @@ namespace gz
148148
/// \return The camera optical frame
149149
public: const std::string& OpticalFrameId() const;
150150

151+
/// \brief Set camera lens intrinsics and projection based on
152+
/// values from SDF. If the camera SDF does not contain intrinsic or
153+
/// projection parameters, the camera will not be updated. Instead, the
154+
/// camera SDF will be updated with intrinsic and projection values
155+
/// computed manually from current camera intrinsic properties.
156+
/// \param[in] _camera Camera to set intrinsic and projection params.
157+
/// \param[in,out] _cameraSdf Camera sdf with intrinisc and projection
158+
/// parameters.
159+
public: void UpdateLensIntrinsicsAndProjection(
160+
rendering::CameraPtr _camera, sdf::Camera &_cameraSdf);
161+
151162
/// \brief Advertise camera info topic.
152163
/// \return True if successful.
153164
protected: bool AdvertiseInfo();

‎src/BoundingBoxCameraSensor.cc

+8-3
Original file line numberDiff line numberDiff line change
@@ -280,9 +280,6 @@ bool BoundingBoxCameraSensor::CreateCamera()
280280
return false;
281281
}
282282

283-
// Camera Info Msg
284-
this->PopulateInfo(sdfCamera);
285-
286283
if (!this->dataPtr->rgbCamera)
287284
{
288285
// Create rendering camera
@@ -333,6 +330,14 @@ bool BoundingBoxCameraSensor::CreateCamera()
333330
this->AddSensor(this->dataPtr->boundingboxCamera);
334331
this->AddSensor(this->dataPtr->rgbCamera);
335332

333+
this->UpdateLensIntrinsicsAndProjection(this->dataPtr->rgbCamera,
334+
*sdfCamera);
335+
this->UpdateLensIntrinsicsAndProjection(this->dataPtr->boundingboxCamera,
336+
*sdfCamera);
337+
338+
// Camera Info Msg
339+
this->PopulateInfo(sdfCamera);
340+
336341
// Create the directory to store frames
337342
if (sdfCamera->SaveFrames())
338343
{

‎src/CameraSensor.cc

+82-74
Original file line numberDiff line numberDiff line change
@@ -291,38 +291,8 @@ bool CameraSensor::CreateCamera()
291291
break;
292292
}
293293

294-
// Update the DOM object intrinsics to have consistent
295-
// intrinsics between ogre camera and camera_info msg
296-
if(!cameraSdf->HasLensIntrinsics())
297-
{
298-
auto intrinsicMatrix =
299-
gz::rendering::projectionToCameraIntrinsic(
300-
this->dataPtr->camera->ProjectionMatrix(),
301-
this->dataPtr->camera->ImageWidth(),
302-
this->dataPtr->camera->ImageHeight()
303-
);
304-
305-
cameraSdf->SetLensIntrinsicsFx(intrinsicMatrix(0, 0));
306-
cameraSdf->SetLensIntrinsicsFy(intrinsicMatrix(1, 1));
307-
cameraSdf->SetLensIntrinsicsCx(intrinsicMatrix(0, 2));
308-
cameraSdf->SetLensIntrinsicsCy(intrinsicMatrix(1, 2));
309-
}
310-
// set custom projection matrix based on intrinsics param specified in sdf
311-
else
312-
{
313-
double fx = cameraSdf->LensIntrinsicsFx();
314-
double fy = cameraSdf->LensIntrinsicsFy();
315-
double cx = cameraSdf->LensIntrinsicsCx();
316-
double cy = cameraSdf->LensIntrinsicsCy();
317-
double s = cameraSdf->LensIntrinsicsSkew();
318-
auto projectionMatrix = CameraSensorPrivate::BuildProjectionMatrix(
319-
this->dataPtr->camera->ImageWidth(),
320-
this->dataPtr->camera->ImageHeight(),
321-
fx, fy, cx, cy, s,
322-
this->dataPtr->camera->NearClipPlane(),
323-
this->dataPtr->camera->FarClipPlane());
324-
this->dataPtr->camera->SetProjectionMatrix(projectionMatrix);
325-
}
294+
this->UpdateLensIntrinsicsAndProjection(this->dataPtr->camera,
295+
*cameraSdf);
326296

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

@@ -336,48 +306,6 @@ bool CameraSensor::CreateCamera()
336306
this->dataPtr->saveImage = true;
337307
}
338308

339-
// Update the DOM object intrinsics to have consistent
340-
// projection matrix values between ogre camera and camera_info msg
341-
// If these values are not defined in the SDF then we need to update
342-
// these values to something reasonable. The projection matrix is
343-
// the cumulative effect of intrinsic and extrinsic parameters
344-
if(!cameraSdf->HasLensProjection())
345-
{
346-
// Note that the matrix from Ogre via camera->ProjectionMatrix() has a
347-
// different format than the projection matrix used in SDFormat.
348-
// This is why they are converted using projectionToCameraIntrinsic.
349-
// The resulting matrix is the intrinsic matrix, but since the user has
350-
// not overridden the values, this is also equal to the projection matrix.
351-
auto intrinsicMatrix =
352-
gz::rendering::projectionToCameraIntrinsic(
353-
this->dataPtr->camera->ProjectionMatrix(),
354-
this->dataPtr->camera->ImageWidth(),
355-
this->dataPtr->camera->ImageHeight()
356-
);
357-
cameraSdf->SetLensProjectionFx(intrinsicMatrix(0, 0));
358-
cameraSdf->SetLensProjectionFy(intrinsicMatrix(1, 1));
359-
cameraSdf->SetLensProjectionCx(intrinsicMatrix(0, 2));
360-
cameraSdf->SetLensProjectionCy(intrinsicMatrix(1, 2));
361-
}
362-
// set custom projection matrix based on projection param specified in sdf
363-
else
364-
{
365-
// tx and ty are not used
366-
double fx = cameraSdf->LensProjectionFx();
367-
double fy = cameraSdf->LensProjectionFy();
368-
double cx = cameraSdf->LensProjectionCx();
369-
double cy = cameraSdf->LensProjectionCy();
370-
double s = 0;
371-
372-
auto projectionMatrix = CameraSensorPrivate::BuildProjectionMatrix(
373-
this->dataPtr->camera->ImageWidth(),
374-
this->dataPtr->camera->ImageHeight(),
375-
fx, fy, cx, cy, s,
376-
this->dataPtr->camera->NearClipPlane(),
377-
this->dataPtr->camera->FarClipPlane());
378-
this->dataPtr->camera->SetProjectionMatrix(projectionMatrix);
379-
}
380-
381309
// Populate camera info topic
382310
this->PopulateInfo(cameraSdf);
383311

@@ -886,6 +814,86 @@ const std::string& CameraSensor::OpticalFrameId() const
886814
return this->dataPtr->opticalFrameId;
887815
}
888816

817+
//////////////////////////////////////////////////
818+
void CameraSensor::UpdateLensIntrinsicsAndProjection(
819+
rendering::CameraPtr _camera, sdf::Camera &_cameraSdf)
820+
{
821+
// Update the DOM object intrinsics to have consistent
822+
// intrinsics between ogre camera and camera_info msg
823+
if(!_cameraSdf.HasLensIntrinsics())
824+
{
825+
auto intrinsicMatrix =
826+
gz::rendering::projectionToCameraIntrinsic(
827+
_camera->ProjectionMatrix(),
828+
_camera->ImageWidth(),
829+
_camera->ImageHeight()
830+
);
831+
832+
_cameraSdf.SetLensIntrinsicsFx(intrinsicMatrix(0, 0));
833+
_cameraSdf.SetLensIntrinsicsFy(intrinsicMatrix(1, 1));
834+
_cameraSdf.SetLensIntrinsicsCx(intrinsicMatrix(0, 2));
835+
_cameraSdf.SetLensIntrinsicsCy(intrinsicMatrix(1, 2));
836+
}
837+
// set custom projection matrix based on intrinsics param specified in sdf
838+
else
839+
{
840+
double fx = _cameraSdf.LensIntrinsicsFx();
841+
double fy = _cameraSdf.LensIntrinsicsFy();
842+
double cx = _cameraSdf.LensIntrinsicsCx();
843+
double cy = _cameraSdf.LensIntrinsicsCy();
844+
double s = _cameraSdf.LensIntrinsicsSkew();
845+
auto projectionMatrix = CameraSensorPrivate::BuildProjectionMatrix(
846+
_camera->ImageWidth(),
847+
_camera->ImageHeight(),
848+
fx, fy, cx, cy, s,
849+
_camera->NearClipPlane(),
850+
_camera->FarClipPlane());
851+
_camera->SetProjectionMatrix(projectionMatrix);
852+
}
853+
854+
// Update the DOM object intrinsics to have consistent
855+
// projection matrix values between ogre camera and camera_info msg
856+
// If these values are not defined in the SDF then we need to update
857+
// these values to something reasonable. The projection matrix is
858+
// the cumulative effect of intrinsic and extrinsic parameters
859+
if(!_cameraSdf.HasLensProjection())
860+
{
861+
// Note that the matrix from Ogre via camera->ProjectionMatrix() has a
862+
// different format than the projection matrix used in SDFormat.
863+
// This is why they are converted using projectionToCameraIntrinsic.
864+
// The resulting matrix is the intrinsic matrix, but since the user has
865+
// not overridden the values, this is also equal to the projection matrix.
866+
auto intrinsicMatrix =
867+
gz::rendering::projectionToCameraIntrinsic(
868+
_camera->ProjectionMatrix(),
869+
_camera->ImageWidth(),
870+
_camera->ImageHeight()
871+
);
872+
_cameraSdf.SetLensProjectionFx(intrinsicMatrix(0, 0));
873+
_cameraSdf.SetLensProjectionFy(intrinsicMatrix(1, 1));
874+
_cameraSdf.SetLensProjectionCx(intrinsicMatrix(0, 2));
875+
_cameraSdf.SetLensProjectionCy(intrinsicMatrix(1, 2));
876+
}
877+
// set custom projection matrix based on projection param specified in sdf
878+
else
879+
{
880+
// tx and ty are not used
881+
double fx = _cameraSdf.LensProjectionFx();
882+
double fy = _cameraSdf.LensProjectionFy();
883+
double cx = _cameraSdf.LensProjectionCx();
884+
double cy = _cameraSdf.LensProjectionCy();
885+
double s = 0;
886+
887+
auto projectionMatrix = CameraSensorPrivate::BuildProjectionMatrix(
888+
_camera->ImageWidth(),
889+
_camera->ImageHeight(),
890+
fx, fy, cx, cy, s,
891+
_camera->NearClipPlane(),
892+
_camera->FarClipPlane());
893+
_camera->SetProjectionMatrix(projectionMatrix);
894+
}
895+
}
896+
889897
//////////////////////////////////////////////////
890898
math::Matrix4d CameraSensorPrivate::BuildProjectionMatrix(
891899
double _imageWidth, double _imageHeight,

‎src/DepthCameraSensor.cc

+6-2
Original file line numberDiff line numberDiff line change
@@ -327,7 +327,7 @@ bool DepthCameraSensor::Load(const sdf::Sensor &_sdf)
327327
//////////////////////////////////////////////////
328328
bool DepthCameraSensor::CreateCamera()
329329
{
330-
const sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor();
330+
sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor();
331331

332332
if (!cameraSdf)
333333
{
@@ -341,7 +341,6 @@ bool DepthCameraSensor::CreateCamera()
341341
double far = cameraSdf->FarClip();
342342
double near = cameraSdf->NearClip();
343343

344-
this->PopulateInfo(cameraSdf);
345344

346345
this->dataPtr->depthCamera = this->Scene()->CreateDepthCamera(
347346
this->Name());
@@ -404,6 +403,9 @@ bool DepthCameraSensor::CreateCamera()
404403

405404
this->Scene()->RootVisual()->AddChild(this->dataPtr->depthCamera);
406405

406+
this->UpdateLensIntrinsicsAndProjection(this->dataPtr->depthCamera,
407+
*cameraSdf);
408+
407409
// Create the directory to store frames
408410
if (cameraSdf->SaveFrames())
409411
{
@@ -412,6 +414,8 @@ bool DepthCameraSensor::CreateCamera()
412414
this->dataPtr->saveImage = true;
413415
}
414416

417+
this->PopulateInfo(cameraSdf);
418+
415419
this->dataPtr->depthConnection =
416420
this->dataPtr->depthCamera->ConnectNewDepthFrame(
417421
std::bind(&DepthCameraSensor::OnNewDepthFrame, this,

‎src/RgbdCameraSensor.cc

+6-3
Original file line numberDiff line numberDiff line change
@@ -277,16 +277,14 @@ bool RgbdCameraSensor::Load(const sdf::Sensor &_sdf)
277277
//////////////////////////////////////////////////
278278
bool RgbdCameraSensor::CreateCameras()
279279
{
280-
const sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor();
280+
sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor();
281281

282282
if (!cameraSdf)
283283
{
284284
gzerr << "Unable to access camera SDF element\n";
285285
return false;
286286
}
287287

288-
this->PopulateInfo(cameraSdf);
289-
290288
int width = cameraSdf->ImageWidth();
291289
int height = cameraSdf->ImageHeight();
292290

@@ -379,6 +377,11 @@ bool RgbdCameraSensor::CreateCameras()
379377

380378
this->Scene()->RootVisual()->AddChild(this->dataPtr->depthCamera);
381379

380+
this->UpdateLensIntrinsicsAndProjection(this->dataPtr->depthCamera,
381+
*cameraSdf);
382+
383+
this->PopulateInfo(cameraSdf);
384+
382385
this->dataPtr->depthConnection =
383386
this->dataPtr->depthCamera->ConnectNewDepthFrame(
384387
std::bind(&RgbdCameraSensorPrivate::OnNewDepthFrame,

‎src/SegmentationCameraSensor.cc

+10-4
Original file line numberDiff line numberDiff line change
@@ -275,7 +275,7 @@ void SegmentationCameraSensor::SetScene(
275275
/////////////////////////////////////////////////
276276
bool SegmentationCameraSensor::CreateCamera()
277277
{
278-
const auto sdfCamera = this->dataPtr->sdfSensor.CameraSensor();
278+
auto sdfCamera = this->dataPtr->sdfSensor.CameraSensor();
279279
if (!sdfCamera)
280280
{
281281
gzerr << "Unable to access camera SDF element\n";
@@ -304,9 +304,6 @@ bool SegmentationCameraSensor::CreateCamera()
304304
}
305305
}
306306

307-
// Camera Info Msg
308-
this->PopulateInfo(sdfCamera);
309-
310307
// Save frames properties
311308
if (sdfCamera->SaveFrames())
312309
{
@@ -379,6 +376,9 @@ bool SegmentationCameraSensor::CreateCamera()
379376
// Add the rendering sensor to handle its render
380377
this->AddSensor(this->dataPtr->camera);
381378

379+
this->UpdateLensIntrinsicsAndProjection(this->dataPtr->camera,
380+
*sdfCamera);
381+
382382
// Add the rgb camera only if we want to generate dataset / save samples
383383
if (this->dataPtr->saveSamples)
384384
{
@@ -396,8 +396,14 @@ bool SegmentationCameraSensor::CreateCamera()
396396
this->Scene()->RootVisual()->AddChild(this->dataPtr->rgbCamera);
397397
this->AddSensor(this->dataPtr->rgbCamera);
398398
this->dataPtr->image = this->dataPtr->rgbCamera->CreateImage();
399+
400+
this->UpdateLensIntrinsicsAndProjection(this->dataPtr->rgbCamera,
401+
*sdfCamera);
399402
}
400403

404+
// Camera Info Msg
405+
this->PopulateInfo(sdfCamera);
406+
401407
// Connection to receive the segmentation buffer
402408
this->dataPtr->newSegmentationConnection =
403409
this->dataPtr->camera->ConnectNewSegmentationFrame(

‎src/ThermalCameraSensor.cc

+6-3
Original file line numberDiff line numberDiff line change
@@ -247,7 +247,7 @@ bool ThermalCameraSensor::Load(const sdf::Sensor &_sdf)
247247
//////////////////////////////////////////////////
248248
bool ThermalCameraSensor::CreateCamera()
249249
{
250-
const sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor();
250+
sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor();
251251

252252
if (!cameraSdf)
253253
{
@@ -263,8 +263,6 @@ bool ThermalCameraSensor::CreateCamera()
263263
double farPlane = cameraSdf->FarClip();
264264
double nearPlane = cameraSdf->NearClip();
265265

266-
this->PopulateInfo(cameraSdf);
267-
268266
this->dataPtr->thermalCamera = this->Scene()->CreateThermalCamera(
269267
this->Name());
270268
this->dataPtr->thermalCamera->SetImageWidth(width);
@@ -346,6 +344,11 @@ bool ThermalCameraSensor::CreateCamera()
346344

347345
this->Scene()->RootVisual()->AddChild(this->dataPtr->thermalCamera);
348346

347+
this->UpdateLensIntrinsicsAndProjection(this->dataPtr->thermalCamera,
348+
*cameraSdf);
349+
350+
this->PopulateInfo(cameraSdf);
351+
349352
// Create the directory to store frames
350353
if (cameraSdf->SaveFrames())
351354
{

0 commit comments

Comments
 (0)