Added camera intrinsics (fx, fy, cx, cy, s).

#2058 Merged at da13146
Repository
carlosmccosta
Branch
camera_intrinsics
Repository
osrf
Branch
default
Author
  1. Carlos Miguel Correia da Costa
Reviewers
Description

Useful for simulating realistic cameras and also for emulating projectors as cameras (for spatial augmented reality).

Tested in ROS Melodic in Ubuntu 18.04.

Related with:

Related work:

For testing this PR:

For loading the testing world and viewing the camera image using the gazebo_ros_pkgs, I created a branch which allows to dynamically change the camera parameters using a CameraInfo message:

roslaunch gazebo_ros empty_world.launch world_name:=/folder_path/camera_intrinsics.world
rqt_image_view /camera_sensor/image_raw

Should I include the testing world in gazebo/worlds ?

For dynamically changing the camera intrinsics and distortion parameters (example):

rostopic pub /camera_sensor/set_camera_info sensor_msgs/CameraInfo "
header:
seq: 30
stamp:
secs: 228
nsecs: 138000000
frame_id: "camera_link"
height: 800
width: 1280
distortion_model: "plumb_bob"
D: [-0.03576145443934453, 0.2722152245603684, -0.0017235085650303857, -0.002987164047049061, 0.0]
K: [1945.035121562, 0.0, 613.836478629, 0.0, 1945.517200549381, -5.52387198283725, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [1945.035121562, 0.0, 613.836478629, -0.0, 0.0, 1945.517200549381, -5.52387198283725, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi:
x_offset: 0
y_offset: 0
height: 0
width: 0
do_rectify: False"

When changing the intrinsics values (K in the msg above), the rendered imaged should have a behavior like this:

P.S.

For compiling the gazebo and sdformat with their dependencies retrieved from the gazebo ppa, I had to disable the ignition-fuel_tools in cmake/SearchForStuff.cmake

Comments (21)

  1. Ian Chen

    thanks for the contributions!

    There are a few coding style errors. Can you run sh tools/code_check.sh from gazebo root directory? that will output the errors. Some coding style guidelines can be found here

    1. Carlos Miguel Correia da Costa author

      Corrected code style. Currently sh tools/code_check.sh gives no errors.

  2. Carlos Miguel Correia da Costa author

    I added a testing world with a chess board in: https://github.com/inesc-tec-robotics/gazebo_projection_mapping/blob/hydro-devel/worlds/camera_intrinsics.world

    Some images showing the affect of increasing and decreasing each camera intrinsic parameters are available at: https://github.com/inesc-tec-robotics/gazebo_projection_mapping/tree/hydro-devel/docs/camera_intrinsics

    If you need further improvements let me known :) Have a nice day :)

  3. Kevin Allen

    +1 Just wanted to bump this (I can resolve conflicts if original author would not like to). Would be very convient for simulating robot’s cameras which have been calibrated and have known intrinsics. It is currently difficult / complicated to simulate ROS code using image/projection geometry with gazebo.

    1. Carlos Miguel Correia da Costa author

      Good afternoon 🙂

      I can resolve the conflicts, no problem (from time to time I sync this fork with the gazebo master while waiting for feedback / integration of this pull request).

      Simulating the intrinsics of the cameras also has other applications besides testing vision systems.

      Namely projection of information into the environment with high accuracy.

      Example for the cooperative assembly of a car engine:

      https://www.youtube.com/watch?v=QWQuF1o5Z9Y

      Have a nice day :)

      1. Kevin Allen

        I appreciate it! Out of curiosity, have you tested how this works when the distortion parameters are also set?

        1. Carlos Miguel Correia da Costa author

          I have tested both the distortion simulation inside gazebo and also using the image_proc and both usually work well:

          For more extreme / large distortions the gazebo implementation was crashing while the image_proc seemed to continue to work.

      2. Carlos Miguel Correia da Costa author

        Good night 🙂

        I resolved the conflicts and rebased the 3 PRs on top of the current default branches for gazebo / sdformat and the melodic-devel for the gazebo_ros_pkgs.

        A test world file and commands for dynamically changing the intrinsic / distortion parameters are available in the gazebo_ros_pkgs PR in the link below.

        Let me known of any suggestions for improvements / corrections :)

        gazebo: https://bitbucket.org/osrf/gazebo/pull-requests/2058/added-camera-intrinsics-fx-fy-cx-cy-s/

        sdformat: https://bitbucket.org/osrf/sdformat/pull-requests/248/added-camera-intrinsics-fx-fy-cx-cy-s

        gazebo_ros_pkgs: https://github.com/ros-simulation/gazebo_ros_pkgs/pull/767

  4. Christian Rauch

    I would like to see this merged. It's an important feature for the camera sensor.
    Can you resolve the merge conflicts and rebase on master/default?

    1. Carlos Miguel Correia da Costa author

      Good afternoon 🙂

      I can resolve the conflicts at the end of the day, no problem.

      I would also really appreciated if this PR and the others associated with it could be merged.

      I think it would be a valuable addition to anyone that needs to model the intrinsic parameters of cameras that were calibrated using OpenCV.

      Have a nice day 🙂

      1. Carlos Miguel Correia da Costa author

        Good night 🙂

        Rebased the camera_intrinsics branch on top of the default for fixing merge conflicts.

        Let me know if further improvements are needed.

        Have a nice day 🙂

    1. Ian Chen

      by the way, this works well for me and it’s relatively low risk to integrate as it does not change the default camera behavior. We can consider merging this for gazebo10 release.

  5. Ian Chen

    Here’re some very minor coding style changes. Could you apply this patch?

    diff -r abb20f3a0c28 gazebo/rendering/Camera.cc
    --- a/gazebo/rendering/Camera.cc        Thu Jul 12 00:44:27 2018 +0100
    +++ b/gazebo/rendering/Camera.cc        Tue Jan 15 17:06:04 2019 -0800
    @@ -191,7 +191,7 @@
         this->dataPtr->distortion->Load(this->sdf->GetElement("distortion"));
       }
    
    -  LoadCameraIntrinsics();
    +  this->LoadCameraIntrinsics();
     }
    
     //////////////////////////////////////////////////
    @@ -203,7 +203,7 @@
         if (sdfLens->HasElement("intrinsics"))
         {
           sdf::ElementPtr sdfIntrinsics = sdfLens->GetElement("intrinsics");
    -      UpdateCameraIntrinsics(
    +      this->UpdateCameraIntrinsics(
               sdfIntrinsics->Get<double>("fx"),
               sdfIntrinsics->Get<double>("fy"),
               sdfIntrinsics->Get<double>("cx"),
    @@ -229,15 +229,17 @@
         clipFar = clipElem->Get<double>("far");
       }
    
    -  this->cameraProjectiveMatrix = BuildProjectiveMatrix(
    +  this->cameraProjectiveMatrix = this->BuildProjectiveMatrix(
         this->imageWidth, this->imageHeight,
         _cameraIntrinsicsFx, _cameraIntrinsicsFy,
         _cameraIntrinsicsCx, _cameraIntrinsicsCy,
         _cameraIntrinsicsS, clipNear, clipFar);
    
    -  if (this->camera != NULL)
    +  if (this->camera != nullptr)
    +  {
         this->camera->setCustomProjectionMatrix(true,
             Conversions::Convert(cameraProjectiveMatrix));
    +  }
    
       this->cameraUsingIntrinsics = true;
     }
    @@ -305,9 +307,9 @@
         const double _intrinsicsS,
         const double _clipNear, const double _clipFar)
     {
    -  return BuildNDCMatrix(
    +  return this->BuildNDCMatrix(
                0, _imageWidth, 0, _imageHeight, _clipNear, _clipFar) *
    -         BuildPerspectiveMatrix(
    +         this->BuildPerspectiveMatrix(
                _intrinsicsFx, _intrinsicsFy, _intrinsicsCx, _intrinsicsCy,
                _intrinsicsS, _clipNear, _clipFar);
     }
    
    1. Carlos Miguel Correia da Costa author

      BuildNDCMatrixand BuildPerspectiveMatrix are static functions.

      Should I use:
      + return Camera::BuildNDCMatrix(
      0, _imageWidth, 0, _imageHeight, _clipNear, _clipFar) *
      - BuildPerspectiveMatrix(
      + Camera::BuildPerspectiveMatrix(
      _intrinsicsFx, _intrinsicsFy, _intrinsicsCx, _intrinsicsCy,
      _intrinsicsS, _clipNear, _clipFar);

      1. Ian Chen

        I think we've used both versions before but maybe Camera:: so it makes it clear that it’s a static function.