Inconsistent 'horizontal_fov' and 'focal' parameters

[cyberguy42]

While driving a robot around in gazebo, I noticed that the pointcloud produced from the realsense depth image was incorrect:

All I did between taking these screenshots was rotate the robot in place.

The published static map accurately shows the location of walls in the world to within 1 cell, and some custom (and thoroughly tested) code publishes the necessary transforms such that the pose of the robot in the map frame exactly matches its pose in gazebo. In short, pointclouds should align with the lines on the map and the observed problem is not due to odometry errors. Based on this knowledge, and the fact that I have not seen these kinds of errors when running the exact same world with a Kinect on a Turtlebot, I concluded that the problem must be within realsense_gazebo_plugin. Additional configuration info: Gazebo 7; realsense_gazebo_plugin: 244629d536128a4bdcb968b7ce9555e3baae7b50

I checked the source code and found where the focal length is defined (463.889) and calculated the corresponding horizontal fov: 2*arctan(640/(2*463.889)) = 1.208. However, the default value for horizontal_fov in urdf/realsense-RS200.macro.xacro is 1.047.

I replaced 1.047 with 1.208 for all of the cameras and reloaded my simulation. The result:

Based on the above analysis, I feel reasonably confident in asserting that the provided values for the parameters are inconsistent and that at least one of them needs to be changed. Ideally, focal would be determined by horizontal_fov, though I'm not certain how that would be done.

[SyrianSpock]

Hi @cyberguy42,

Your analysis is correct. Thanks a lot for the thorough investigation.

I have addressed this by computing the focal length (published over ROS) from the horizontal FOV (given by the Gazebo rendering object). So there is a single source of truth now: the horizontal FOV value set in URDF. Can you please try this branch?

[cyberguy42]

No problem, glad I could help. The new branch seems to fix the problem, thank you for acting so promptly! I did have to make a few minor edits in order to compile (more on that below).

With horizontal FOV set to 1.047, everything looks good:

With horizontal FOV set to 1.2, the pointcloud changes as expected:

Edits I'm guessing that the new branch is intended for Gazebo 9, while I am using Gazebo 7 (changeset 35362:68d94d3cce38 to be precise). In any case, I only had to make a few very small changes to get everything to compile. Here is the diff:

diff --git a/src/RealSensePlugin.cpp b/src/RealSensePlugin.cpp
index 29fc2cd..e2b300f 100644
--- a/src/RealSensePlugin.cpp
+++ b/src/RealSensePlugin.cpp
@@ -120,7 +120,7 @@ void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)

   // Setup Transport Node
   this->transportNode = transport::NodePtr(new transport::Node());
-  this->transportNode->Init(this->world->Name());
+  this->transportNode->Init(this->world->GetName());

   // Setup Publishers
   std::string rsTopicRoot =
@@ -166,7 +166,7 @@ void RealSensePlugin::OnNewFrame(const rendering::CameraPtr cam,
   msgs::ImageStamped msg;

   // Set Simulation Time
-  msgs::Set(msg.mutable_time(), this->world->SimTime());
+  msgs::Set(msg.mutable_time(), this->world->GetSimTime());

   // Set Image Dimensions
   msg.mutable_image()->set_width(cam->ImageWidth());
@@ -215,7 +215,7 @@ void RealSensePlugin::OnNewDepthFrame()
   }

   // Pack realsense scaled depth map
-  msgs::Set(msg.mutable_time(), this->world->SimTime());
+  msgs::Set(msg.mutable_time(), this->world->GetSimTime());
   msg.mutable_image()->set_width(this->depthCam->ImageWidth());
   msg.mutable_image()->set_height(this->depthCam->ImageHeight());
   msg.mutable_image()->set_pixel_format(common::Image::L_INT16);
diff --git a/src/gazebo_ros_realsense.cpp b/src/gazebo_ros_realsense.cpp
index c795888..70ead6c 100644
--- a/src/gazebo_ros_realsense.cpp
+++ b/src/gazebo_ros_realsense.cpp
@@ -51,7 +51,7 @@ void GazeboRosRealsense::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
 void GazeboRosRealsense::OnNewFrame(const rendering::CameraPtr cam,
                                     const transport::PublisherPtr pub)
 {
-  common::Time current_time = this->world->SimTime();
+  common::Time current_time = this->world->GetSimTime();

   // identify camera
   std::string camera_id = extractCameraName(cam->Name());
@@ -96,7 +96,7 @@ void GazeboRosRealsense::OnNewFrame(const rendering::CameraPtr cam,
 void GazeboRosRealsense::OnNewDepthFrame()
 {
   // get current time
-  common::Time current_time = this->world->SimTime();
+  common::Time current_time = this->world->GetSimTime();

   RealSensePlugin::OnNewDepthFrame();

Thanks again!

[SyrianSpock]

Good to know, thanks. Indeed I forgot to mention this change was intended for Gazebo 9.

I merged the fix to master and kinetic-devel branches