Skip to content

Commit

Permalink
Added option to specify x and y components of camera focal length.
Browse files Browse the repository at this point in the history
  • Loading branch information
@carlosmccosta
carlosmccosta committed Dec 17, 2015
1 parent c9ad4df commit 50899ac
Show file tree
Hide file tree
Showing 3 changed files with 42 additions and 19 deletions.
3 changes: 2 additions & 1 deletion gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,7 +147,8 @@ namespace gazebo
protected: double cx_prime_;
protected: double cx_;
protected: double cy_;
protected: double focal_length_;
protected: double focal_length_x_;
protected: double focal_length_y_;
protected: double hack_baseline_;
protected: double distortion_k1_;
protected: double distortion_k2_;
Expand Down
48 changes: 35 additions & 13 deletions gazebo_plugins/src/gazebo_ros_camera_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -175,13 +175,34 @@ void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent,
else
this->cy_ = this->sdf->Get<double>("Cy");

if (!this->sdf->HasElement("focalLength"))
if (this->sdf->HasElement("focalLength"))
{
ROS_DEBUG("Camera plugin missing <focalLength>, defaults to 0");
this->focal_length_= 0;
this->focal_length_x_ = this->sdf->Get<double>("focalLength");
this->focal_length_y_ = this->focal_length_x_;
}

if (!this->sdf->HasElement("focalLength_x"))
{
if (!this->sdf->HasElement("focalLength"))
{
ROS_DEBUG("Camera plugin missing <focalLengthX>, defaults to 0");
this->focal_length_x_= 0;
}
}
else
this->focal_length_x_ = this->sdf->Get<double>("focalLengthX");

if (!this->sdf->HasElement("focalLength_y"))
{
if (!this->sdf->HasElement("focalLength"))
{
ROS_DEBUG("Camera plugin missing <focalLengthY>, defaults to 0");
this->focal_length_y_= 0;
}
}
else
this->focal_length_ = this->sdf->Get<double>("focalLength");
this->focal_length_y_ = this->sdf->Get<double>("focalLengthY");


if (!this->sdf->HasElement("hackBaseline"))
{
Expand Down Expand Up @@ -472,22 +493,23 @@ void GazeboRosCameraUtils::Init()
(static_cast<double>(this->width_)) /
(2.0 * tan(this->camera_->GetHFOV().Radian() / 2.0));

if (this->focal_length_ == 0)
if (this->focal_length_x_ == 0 || this->focal_length_y_ == 0)
{
this->focal_length_ = computed_focal_length;
this->focal_length_x_ = computed_focal_length;
this->focal_length_y_ = computed_focal_length;
}
else
{
// check against float precision
if (!gazebo::math::equal(this->focal_length_, computed_focal_length))
if (!gazebo::math::equal(this->focal_length_x_, computed_focal_length))
{
ROS_WARN("The <focal_length>[%f] you have provided for camera_ [%s]"
" is inconsistent with specified image_width [%d] and"
" HFOV [%f]. Please double check to see that"
" focal_length = width_ / (2.0 * tan(HFOV/2.0)),"
" the explected focal_lengtth value is [%f],"
" please update your camera_ model description accordingly.",
this->focal_length_, this->parentSensor_->GetName().c_str(),
this->focal_length_x_, this->parentSensor_->GetName().c_str(),
this->width_, this->camera_->GetHFOV().Radian(),
computed_focal_length);
}
Expand All @@ -511,11 +533,11 @@ void GazeboRosCameraUtils::Init()
camera_info_msg.D[3] = this->distortion_t1_;
camera_info_msg.D[4] = this->distortion_t2_;
// original camera_ matrix
camera_info_msg.K[0] = this->focal_length_;
camera_info_msg.K[0] = this->focal_length_x_;
camera_info_msg.K[1] = 0.0;
camera_info_msg.K[2] = this->cx_;
camera_info_msg.K[3] = 0.0;
camera_info_msg.K[4] = this->focal_length_;
camera_info_msg.K[4] = this->focal_length_y_;
camera_info_msg.K[5] = this->cy_;
camera_info_msg.K[6] = 0.0;
camera_info_msg.K[7] = 0.0;
Expand All @@ -532,12 +554,12 @@ void GazeboRosCameraUtils::Init()
camera_info_msg.R[8] = 1.0;
// camera_ projection matrix (same as camera_ matrix due
// to lack of distortion/rectification) (is this generated?)
camera_info_msg.P[0] = this->focal_length_;
camera_info_msg.P[0] = this->focal_length_x_;
camera_info_msg.P[1] = 0.0;
camera_info_msg.P[2] = this->cx_;
camera_info_msg.P[3] = -this->focal_length_ * this->hack_baseline_;
camera_info_msg.P[3] = -this->focal_length_x_ * this->hack_baseline_;
camera_info_msg.P[4] = 0.0;
camera_info_msg.P[5] = this->focal_length_;
camera_info_msg.P[5] = this->focal_length_y_;
camera_info_msg.P[6] = this->cy_;
camera_info_msg.P[7] = 0.0;
camera_info_msg.P[8] = 0.0;
Expand Down
10 changes: 5 additions & 5 deletions gazebo_plugins/src/gazebo_ros_prosilica.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -235,11 +235,11 @@ void GazeboRosProsilica::pollCallback(polled_camera::GetPolledImage::Request& re
this->roiCameraInfoMsg->D[3] = this->distortion_t1_;
this->roiCameraInfoMsg->D[4] = this->distortion_t2_;
// original camera matrix
this->roiCameraInfoMsg->K[0] = this->focal_length_;
this->roiCameraInfoMsg->K[0] = this->focal_length_x_;
this->roiCameraInfoMsg->K[1] = 0.0;
this->roiCameraInfoMsg->K[2] = this->cx_ - req.roi.x_offset;
this->roiCameraInfoMsg->K[3] = 0.0;
this->roiCameraInfoMsg->K[4] = this->focal_length_;
this->roiCameraInfoMsg->K[4] = this->focal_length_y_;
this->roiCameraInfoMsg->K[5] = this->cy_ - req.roi.y_offset;
this->roiCameraInfoMsg->K[6] = 0.0;
this->roiCameraInfoMsg->K[7] = 0.0;
Expand All @@ -255,12 +255,12 @@ void GazeboRosProsilica::pollCallback(polled_camera::GetPolledImage::Request& re
this->roiCameraInfoMsg->R[7] = 0.0;
this->roiCameraInfoMsg->R[8] = 1.0;
// camera projection matrix (same as camera matrix due to lack of distortion/rectification) (is this generated?)
this->roiCameraInfoMsg->P[0] = this->focal_length_;
this->roiCameraInfoMsg->P[0] = this->focal_length_x_;
this->roiCameraInfoMsg->P[1] = 0.0;
this->roiCameraInfoMsg->P[2] = this->cx_ - req.roi.x_offset;
this->roiCameraInfoMsg->P[3] = -this->focal_length_ * this->hack_baseline_;
this->roiCameraInfoMsg->P[3] = -this->focal_length_x_ * this->hack_baseline_;
this->roiCameraInfoMsg->P[4] = 0.0;
this->roiCameraInfoMsg->P[5] = this->focal_length_;
this->roiCameraInfoMsg->P[5] = this->focal_length_y_;
this->roiCameraInfoMsg->P[6] = this->cy_ - req.roi.y_offset;
this->roiCameraInfoMsg->P[7] = 0.0;
this->roiCameraInfoMsg->P[8] = 0.0;
Expand Down

0 comments on commit 50899ac

Please sign in to comment.